merged upstream/master into sbus2_sensorssbus2_sensors

880 files changed, 47077 insertions, 131717 deletions

diff --git a/.gitignore b/.gitignore
index 3e94cf620..8b09e4783 100644
--- a/.gitignore
+++ b/.gitignore
@@ -29,9 +29,12 @@ mavlink/include/mavlink/v0.9/
 /nuttx-configs/px4io-v1/src/.depend
 /nuttx-configs/px4io-v1/src/Make.dep
 /nuttx-configs/px4io-v1/src/libboard.a
-/NuttX
 /Documentation/doxy.log
 /Documentation/html/
 /Documentation/doxygen*objdb*tmp
 .tags
+tags
 .tags_sorted_by_file
+.pydevproject
+.ropeproject
+*.orig
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 000000000..8436b398e
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,6 @@
+[submodule "mavlink/include/mavlink/v1.0"]
+	path = mavlink/include/mavlink/v1.0
+	url = git://github.com/mavlink/c_library.git
+[submodule "NuttX"]
+	path = NuttX
+	url = git://github.com/PX4/NuttX.git
diff --git a/.ycm_extra_conf.py b/.ycm_extra_conf.py
new file mode 100644
index 000000000..fed5cd28a
--- /dev/null
+++ b/.ycm_extra_conf.py
@@ -0,0 +1,173 @@
+# This file is NOT licensed under the GPLv3, which is the license for the rest
+# of YouCompleteMe.
+#
+# Here's the license text for this file:
+#
+# This is free and unencumbered software released into the public domain.
+#
+# Anyone is free to copy, modify, publish, use, compile, sell, or
+# distribute this software, either in source code form or as a compiled
+# binary, for any purpose, commercial or non-commercial, and by any
+# means.
+#
+# In jurisdictions that recognize copyright laws, the author or authors
+# of this software dedicate any and all copyright interest in the
+# software to the public domain. We make this dedication for the benefit
+# of the public at large and to the detriment of our heirs and
+# successors. We intend this dedication to be an overt act of
+# relinquishment in perpetuity of all present and future rights to this
+# software under copyright law.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+# OTHER DEALINGS IN THE SOFTWARE.
+#
+# For more information, please refer to <http://unlicense.org/>
+
+import os
+import ycm_core
+
+# These are the compilation flags that will be used in case there's no
+# compilation database set (by default, one is not set).
+# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
+flags = [
+'-Wall',
+'-Wextra',
+'-Werror',
+#'-Wc++98-compat',
+'-Wno-long-long',
+'-Wno-variadic-macros',
+'-fexceptions',
+'-DNDEBUG',
+# You 100% do NOT need -DUSE_CLANG_COMPLETER in your flags; only the YCM
+# source code needs it.
+#'-DUSE_CLANG_COMPLETER',
+# THIS IS IMPORTANT! Without a "-std=<something>" flag, clang won't know which
+# language to use when compiling headers. So it will guess. Badly. So C++
+# headers will be compiled as C headers. You don't want that so ALWAYS specify
+# a "-std=<something>".
+# For a C project, you would set this to something like 'c99' instead of
+# 'c++11'.
+'-std=c++11',
+# ...and the same thing goes for the magic -x option which specifies the
+# language that the files to be compiled are written in. This is mostly
+# relevant for c++ headers.
+# For a C project, you would set this to 'c' instead of 'c++'.
+'-x',
+'c++',
+'-undef', # get rid of standard definitions to allow us to include arm math header
+'-I', os.path.join(os.path.expanduser("~"),'gcc-arm-none-eabi-4_7-2013q3/arm-none-eabi/include'),
+'-I', 'Build/px4io-v1_default.build/nuttx-export/include/',
+'-I', 'Build/px4io-v2_default.build/nuttx-export/include/',
+'-I', './NuttX/nuttx/arch/arm/include',
+'-include', './src/include/visibility.h',
+'-I', './src',
+'-I', './src/modules',
+'-I', './src/include',
+'-I', './src/lib',
+'-I', './NuttX',
+]
+
+
+# Set this to the absolute path to the folder (NOT the file!) containing the
+# compile_commands.json file to use that instead of 'flags'. See here for
+# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
+#
+# Most projects will NOT need to set this to anything; you can just change the
+# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
+compilation_database_folder = ''
+
+if os.path.exists( compilation_database_folder ):
+  database = ycm_core.CompilationDatabase( compilation_database_folder )
+else:
+  database = None
+
+SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ]
+
+def DirectoryOfThisScript():
+  return os.path.dirname( os.path.abspath( __file__ ) )
+
+
+def MakeRelativePathsInFlagsAbsolute( flags, working_directory ):
+  if not working_directory:
+    return list( flags )
+  new_flags = []
+  make_next_absolute = False
+  path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ]
+  for flag in flags:
+    new_flag = flag
+
+    if make_next_absolute:
+      make_next_absolute = False
+      if not flag.startswith( '/' ):
+        new_flag = os.path.join( working_directory, flag )
+
+    for path_flag in path_flags:
+      if flag == path_flag:
+        make_next_absolute = True
+        break
+
+      if flag.startswith( path_flag ):
+        path = flag[ len( path_flag ): ]
+        new_flag = path_flag + os.path.join( working_directory, path )
+        break
+
+    if new_flag:
+      new_flags.append( new_flag )
+  return new_flags
+
+
+def IsHeaderFile( filename ):
+  extension = os.path.splitext( filename )[ 1 ]
+  return extension in [ '.h', '.hxx', '.hpp', '.hh' ]
+
+
+def GetCompilationInfoForFile( filename ):
+  # The compilation_commands.json file generated by CMake does not have entries
+  # for header files. So we do our best by asking the db for flags for a
+  # corresponding source file, if any. If one exists, the flags for that file
+  # should be good enough.
+  if IsHeaderFile( filename ):
+    basename = os.path.splitext( filename )[ 0 ]
+    for extension in SOURCE_EXTENSIONS:
+      replacement_file = basename + extension
+      if os.path.exists( replacement_file ):
+        compilation_info = database.GetCompilationInfoForFile(
+          replacement_file )
+        if compilation_info.compiler_flags_:
+          return compilation_info
+    return None
+  return database.GetCompilationInfoForFile( filename )
+
+
+def FlagsForFile( filename, **kwargs ):
+  if database:
+    # Bear in mind that compilation_info.compiler_flags_ does NOT return a
+    # python list, but a "list-like" StringVec object
+    compilation_info = GetCompilationInfoForFile( filename )
+    if not compilation_info:
+      return None
+
+    final_flags = MakeRelativePathsInFlagsAbsolute(
+      compilation_info.compiler_flags_,
+      compilation_info.compiler_working_dir_ )
+
+    # NOTE: This is just for YouCompleteMe; it's highly likely that your project
+    # does NOT need to remove the stdlib flag. DO NOT USE THIS IN YOUR
+    # ycm_extra_conf IF YOU'RE NOT 100% SURE YOU NEED IT.
+    #try:
+    #  final_flags.remove( '-stdlib=libc++' )
+    #except ValueError:
+    #  pass
+  else:
+    relative_to = DirectoryOfThisScript()
+    final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to )
+
+  return {
+    'flags': final_flags,
+    'do_cache': True
+  }
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 000000000..3bf02fbff
--- /dev/null
+++ b/CONTRIBUTING.md
@@ -0,0 +1,44 @@
+# Contributing to PX4 Firmware
+
+We follow the [Github flow](https://guides.github.com/introduction/flow/) development model.
+
+### Fork the project, then clone your repo
+
+First [fork and clone](https://help.github.com/articles/fork-a-repo) the project project.
+
+### Create a feature branch
+
+*Always* branch off master for new features.
+
+```
+git checkout -b mydescriptivebranchname
+```
+
+### Edit and build the code
+
+The [developer guide](http://pixhawk.org/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://pixhawk.org/dev/code_style) when editing files.
+
+### Commit your changes
+
+Always write descriptive commit messages and add a fixes or relates note to them with an [issue number](https://github.com/px4/Firmware/issues) (Github will link these then conveniently)
+
+**Example:**
+
+```
+Change how the attitude controller works
+
+- Fixes rate feed forward
+- Allows a local body rate override
+
+Fixes issue #123
+```
+
+### Test your changes
+
+Since we care about safety, we will regularly ask you for test results. Best is to do a test flight (or bench test where it applies) and upload the logfile from it (on the microSD card in the logs directory) to Google Drive or Dropbox and share the link.
+
+### Push your changes
+
+Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/).
+
+Make sure to provide some testing feedback and if possible the link to a flight log file.
diff --git a/Debug/NuttX b/Debug/NuttX
index 3b95e96b2..d34e9f5b4 100644
--- a/Debug/NuttX
+++ b/Debug/NuttX
@@ -34,10 +34,10 @@ define _showheap
 	else
 		set $MM_ALLOC_BIT = 0x80000000
 	end
-	printf "HEAP %d  %p - %p\n", $index, g_heapstart[$index], g_heapend[$index]
+	printf "HEAP %d  %p - %p\n", $index, g_mmheap.mm_heapstart[$index], g_mmheap.mm_heapend[$index]
 	printf "ptr      size\n"
-	set $node = (char *)g_heapstart[$index] + sizeof(struct mm_allocnode_s)
-	while $node < g_heapend[$index]
+	set $node = (char *)g_mmheap.mm_heapstart[$index] + sizeof(struct mm_allocnode_s)
+	while $node < g_mmheap.mm_heapend[$index]
 		printf "  %p", $node
 		set $nodestruct = (struct mm_allocnode_s *)$node
 		printf "  %u", $nodestruct->size
@@ -47,7 +47,7 @@ define _showheap
 		else
 			set $used = $used + $nodestruct->size
 		end
-		if ($nodestruct->size > g_heapsize) || (($node + $nodestruct->size) > g_heapend[$index])
+		if ($nodestruct->size > g_mmheap.mm_heapsize) || (($node + $nodestruct->size) > g_mmheap.mm_heapend[$index])
 			printf "  (BAD SIZE)"
 		end
 		printf "\n"
@@ -59,7 +59,7 @@ define _showheap
 end
 
 define showheap
-	set $nheaps = sizeof(g_heapstart) / sizeof(g_heapstart[0])
+	set $nheaps = sizeof(g_mmheap.mm_heapstart) / sizeof(g_mmheap.mm_heapstart[0])
 	printf "Printing %d heaps\n", $nheaps
 	set $heapindex = (int)0
 	while $heapindex < $nheaps
diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py
index 093edd0e0..7cc21b99f 100644
--- a/Debug/Nuttx.py
+++ b/Debug/Nuttx.py
@@ -306,12 +306,15 @@ class NX_show_heap (gdb.Command):
 
 	def __init__(self):
 		super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER)
-		if gdb.lookup_type('struct mm_allocnode_s').sizeof == 8:
-			self._allocflag = 0x80000000
-			self._allocnodesize = 8
-		else:
+                struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
+                preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
+                if preceding_size == 2:
 			self._allocflag = 0x8000
-			self._allocnodesize = 4
+                elif preceding_size == 4:
+			self._allocflag = 0x80000000
+                else:
+                        raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
+                self._allocnodesize = struct_mm_allocnode_s.sizeof
 
 	def _node_allocated(self, allocnode):
 		if allocnode['preceding'] & self._allocflag:
@@ -333,7 +336,8 @@ class NX_show_heap (gdb.Command):
 				state = ''
 			else:
 				state = '(free)'
-			print '  {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state)
+			print '  {} {} {}'.format(allocnode.address + self._allocnodesize,
+                                                  self._node_size(allocnode), state)
 			cursor += self._node_size(allocnode) / self._allocnodesize
 
 	def invoke(self, args, from_tty):
diff --git a/Documentation/fw_landing.png b/Documentation/fw_landing.png
new file mode 100644
index 000000000..c1165f16a
--- /dev/null
+++ b/Documentation/fw_landing.png
diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg
index 29d738c39..682c63e47 100644
--- a/Documentation/rc_mode_switch.odg
+++ b/Documentation/rc_mode_switch.odg
diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf
index 856dd55c5..e795f4870 100644
--- a/Documentation/rc_mode_switch.pdf
+++ b/Documentation/rc_mode_switch.pdf
diff --git a/Images/aerocore.prototype b/Images/aerocore.prototype
new file mode 100644
index 000000000..8502a0864
--- /dev/null
+++ b/Images/aerocore.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 19,
+    "magic": "AeroCore",
+    "description": "Firmware for the Gumstix AeroCore board",
+    "image": "",
+    "build_time": 0,
+    "summary": "AEROCORE",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/Makefile b/Makefile
index 0a8562251..7b7c00704 100644
--- a/Makefile
+++ b/Makefile
@@ -104,7 +104,7 @@ DESIRED_FIRMWARES 	 = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
 STAGED_FIRMWARES	 = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
 FIRMWARES		 = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
 
-all:			$(DESIRED_FIRMWARES)
+all:			checksubmodules $(DESIRED_FIRMWARES)
 
 #
 # Copy FIRMWARES into the image directory.
@@ -210,9 +210,16 @@ menuconfig:
 endif
 
 $(NUTTX_SRC):
-	@$(ECHO) ""
-	@$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again."
-	@$(ECHO) ""
+	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
+
+.PHONY: checksubmodules
+checksubmodules:
+	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
+
+.PHONY: updatesubmodules
+updatesubmodules:
+	$(Q) (git submodule init)
+	$(Q) (git submodule update)
 
 #
 # Testing targets
diff --git a/NuttX b/NuttX
new file mode 160000
+Subproject 7e1b97bcf10d8495169eec355988ca5890bfd5d
diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
index ebe8a1a1e..d114fe21a 100644
--- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
@@ -2,44 +2,13 @@
 #
 # HILStar / X-Plane
 #
-# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
+# Lorenz Meier <lm@inf.ethz.ch>
 #
 
-echo "HIL Rascal 110 starting.."
+sh /etc/init.d/rc.fw_defaults
 
-if [ $DO_AUTOCONFIG == yes ]
-then
-	# Set all params here, then disable autoconfig
-	
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 15
-	param set FW_P_LIM_MAX 50
-	param set FW_P_LIM_MIN -50
-	param set FW_P_P 60
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 1.1
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 100
-	param set FW_R_RMAX 100
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 1
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5.0
-	param set FW_T_SINK_MIN 4.0
-	param set FW_Y_ROLLFF 1.1
-	param set FW_L1_PERIOD 16
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
-
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
+echo "X Plane HIL starting.."
 
 set HIL yes
 
-set VEHICLE_TYPE fw
 set MIXER FMU_AERT
diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
index 63798bb3c..c4be16943 100644
--- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -2,30 +2,28 @@
 #
 # Team Blacksheep Discovery Quadcopter
 #
-# Maintainers: Simon Wilks <sjwilks@gmail.com>
+# Anton Babushkin <anton.babushkin@me.com>, Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
 #
 
+sh /etc/init.d/rc.mc_defaults
+
 if [ $DO_AUTOCONFIG == yes ]
 then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 5.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.17
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.006
-	param set MC_YAWPOS_P 0.5
-	param set MC_YAWPOS_I 0.15
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.0
+	# TODO review MC_YAWRATE_I
+	param set MC_ROLL_P 8.0
+	param set MC_ROLLRATE_P 0.07
+	param set MC_ROLLRATE_I 0.05
+	param set MC_ROLLRATE_D 0.0017
+	param set MC_PITCH_P 8.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.1
+	param set MC_PITCHRATE_D 0.0025
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.28
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 fi
 
-set VEHICLE_TYPE mc
 set MIXER FMU_quad_w
 
 set PWM_OUTPUTS 1234
-set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
index 67c24fab3..084dff140 100644
--- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -2,52 +2,31 @@
 #
 # 3DR Iris Quadcopter
 #
-# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton.babushkin@me.com>
 #
 
+sh /etc/init.d/rc.mc_defaults
+
 if [ $DO_AUTOCONFIG == yes ]
 then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 9.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.13
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 0.5
-	param set MC_YAWPOS_I 0.15
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.0
+	# TODO tune roll/pitch separately
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.13
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.004
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.13
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.004
+	param set MC_YAW_P 2.5
+	param set MC_YAWRATE_P 0.25
+	param set MC_YAWRATE_I 0.25
 	param set MC_YAWRATE_D 0.0
-	
-	param set MPC_TILT_MAX 0.5
-	param set MPC_THR_MAX 0.8
-	param set MPC_THR_MIN 0.2
-	param set MPC_XY_D 0
-	param set MPC_XY_P 0.5
-	param set MPC_XY_VEL_D 0
-	param set MPC_XY_VEL_I 0
-	param set MPC_XY_VEL_MAX 3
-	param set MPC_XY_VEL_P 0.2
-	param set MPC_Z_D 0
-	param set MPC_Z_P 1
-	param set MPC_Z_VEL_D 0
-	param set MPC_Z_VEL_I 0.1
-	param set MPC_Z_VEL_MAX 2
-	param set MPC_Z_VEL_P 0.20
 
 	param set BAT_V_SCALING 0.00989
 	param set BAT_C_SCALING 0.0124
 fi
 
-set VEHICLE_TYPE mc
 set MIXER FMU_quad_w
 
 set PWM_OUTPUTS 1234
-set PWM_RATE 400
-set PWM_DISARMED 900
-set PWM_MIN 1000
-set PWM_MAX 2000
diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
new file mode 100644
index 000000000..6179855f6
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
@@ -0,0 +1,35 @@
+#!nsh
+#
+# Steadidrone QU4D
+#
+# Thomas Gubler <thomasgubler@gmail.com>
+# Lorenz Meier <lm@inf.ethz.ch>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	# TODO tune roll/pitch separately
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.13
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.004
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.13
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.004
+	param set MC_YAW_P 0.5
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_D 0.0
+
+	param set BAT_N_CELLS 4
+fi
+
+set MIXER FMU_quad_w
+
+set PWM_MIN 1210
+set PWM_MAX 2100
+
+set PWM_OUTPUTS 1234
diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
index 8c0797d7c..7a7a9542c 100644
--- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
@@ -2,45 +2,11 @@
 #
 # HIL Quadcopter X
 #
-# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton.babushkin@me.com>
 #
 
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	param set MPC_TILT_MAX 0.5
-	param set MPC_THR_MAX 0.8
-	param set MPC_THR_MIN 0.2
-	param set MPC_XY_D 0
-	param set MPC_XY_P 0.5
-	param set MPC_XY_VEL_D 0
-	param set MPC_XY_VEL_I 0
-	param set MPC_XY_VEL_MAX 3
-	param set MPC_XY_VEL_P 0.2
-	param set MPC_Z_D 0
-	param set MPC_Z_P 1
-	param set MPC_Z_VEL_D 0
-	param set MPC_Z_VEL_I 0.1
-	param set MPC_Z_VEL_MAX 2
-	param set MPC_Z_VEL_P 0.20
-fi
+sh /etc/init.d/rc.mc_defaults
 
-set HIL yes
-
-set VEHICLE_TYPE mc
 set MIXER FMU_quad_x
+
+set HIL yes
diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
deleted file mode 100644
index 46da24d35..000000000
--- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
+++ /dev/null
@@ -1,45 +0,0 @@
-#!nsh
-#
-# HIL Rascal 110 (Flightgear)
-#
-# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
-#
-
-echo "HIL Rascal 110 starting.."
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	# Set all params here, then disable autoconfig
-	
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 15
-	param set FW_P_LIM_MAX 50
-	param set FW_P_LIM_MIN -50
-	param set FW_P_P 60
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 1.1
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 100
-	param set FW_R_RMAX 100
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 1
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5.0
-	param set FW_T_SINK_MIN 4.0
-	param set FW_Y_ROLLFF 1.1
-	param set FW_L1_PERIOD 16
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
-
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
-
-set HIL yes
-
-set VEHICLE_TYPE fw
-set MIXER FMU_AERT
diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
index bce3015fc..c47500c7a 100644
--- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
+++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
@@ -2,45 +2,11 @@
 #
 # HIL Quadcopter +
 #
-# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton.babushkin@me.com>
 #
 
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	param set MPC_TILT_MAX 0.5
-	param set MPC_THR_MAX 0.8
-	param set MPC_THR_MIN 0.2
-	param set MPC_XY_D 0
-	param set MPC_XY_P 0.5
-	param set MPC_XY_VEL_D 0
-	param set MPC_XY_VEL_I 0
-	param set MPC_XY_VEL_MAX 3
-	param set MPC_XY_VEL_P 0.2
-	param set MPC_Z_D 0
-	param set MPC_Z_P 1
-	param set MPC_Z_VEL_D 0
-	param set MPC_Z_VEL_I 0.1
-	param set MPC_Z_VEL_MAX 2
-	param set MPC_Z_VEL_P 0.20
-fi
+sh /etc/init.d/rc.mc_defaults
 
-set HIL yes
-
-set VEHICLE_TYPE mc
 set MIXER FMU_quad_+
+
+set HIL yes
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
index 46da24d35..4e3e18326 100644
--- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
+++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
@@ -2,44 +2,13 @@
 #
 # HIL Rascal 110 (Flightgear)
 #
-# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomasgubler@gmail.com>
 #
 
-echo "HIL Rascal 110 starting.."
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	# Set all params here, then disable autoconfig
-	
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 15
-	param set FW_P_LIM_MAX 50
-	param set FW_P_LIM_MIN -50
-	param set FW_P_P 60
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 1.1
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 100
-	param set FW_R_RMAX 100
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 1
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5.0
-	param set FW_T_SINK_MIN 4.0
-	param set FW_Y_ROLLFF 1.1
-	param set FW_L1_PERIOD 16
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
+sh /etc/init.d/rc.fw_defaults
 
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
+echo "HIL Rascal 110 starting.."
 
 set HIL yes
 
-set VEHICLE_TYPE fw
 set MIXER FMU_AERT
diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
new file mode 100644
index 000000000..c753ded23
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
@@ -0,0 +1,47 @@
+#!nsh
+#
+# HIL Malolo 1 (Flightgear)
+#
+# Thomas Gubler <thomasgubler@gmail.com>
+#
+
+sh /etc/init.d/rc.fw_defaults
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	param set FW_AIRSPD_MIN 12
+	param set FW_AIRSPD_TRIM 25
+	param set FW_AIRSPD_MAX 40
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.74
+	param set FW_L1_PERIOD 15
+	param set FW_PR_FF 0.8
+	param set FW_PR_I 0.05
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.1
+	param set FW_P_ROLLFF 0
+	param set FW_RR_FF 0.6
+	param set FW_RR_I 0.02
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.1
+	param set FW_R_LIM 45
+	param set FW_R_RMAX 0
+	param set FW_T_CLMB_MAX 5
+	param set FW_T_HRATE_P 0.02
+	param set FW_T_PTCH_DAMP 0
+	param set FW_T_RLL2THR 15
+	param set FW_T_SINK_MAX 5
+	param set FW_T_SINK_MIN 2
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 3
+	param set FW_T_VERT_ACC 7
+	param set FW_YR_FF 0.0
+	param set FW_YR_I 0
+	param set FW_YR_IMAX 0.2
+	param set FW_YR_P 0.0
+fi
+
+set HIL yes
+
+# Set the AERT mixer for HIL (even if the malolo is a flying wing)
+set MIXER FMU_AERT
diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
index 2dc83a517..daa04a4de 100644
--- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox
+++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
@@ -1,39 +1,13 @@
 #!nsh
 #
-# UNTESTED UNTESTED!
-#
-# Generic 10” Hexa coaxial geometry
+# Generic 10" Hexa coaxial geometry
 #
 # Lorenz Meier <lm@inf.ethz.ch>
 #
 
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
+sh /etc/init.d/rc.mc_defaults
 
-set VEHICLE_TYPE mc
-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
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox
index a55fc5a30..8703f5f2f 100644
--- a/ROMFS/px4fmu_common/init.d/12001_octo_cox
+++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox
@@ -1,37 +1,12 @@
 #!nsh
 #
-# Generic 10” Octo coaxial geometry
+# Generic 10" Octo coaxial geometry
 #
 # Lorenz Meier <lm@inf.ethz.ch>
 #
 
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
+sh /etc/init.d/rc.mc_defaults
 
-set VEHICLE_TYPE mc
 set MIXER FMU_octo_cox
 
 set PWM_OUTPUTS 12345678
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
index 0e5bf60d6..3ab2ac3d1 100644
--- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
@@ -2,38 +2,7 @@
 #
 # MPX EasyStar Plane
 #
-# Maintainers: ???
-#
 
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 15
-	param set FW_P_LIM_MAX 50
-	param set FW_P_LIM_MIN -50
-	param set FW_P_P 60
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 1.1
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 100
-	param set FW_R_RMAX 100
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 1
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5.0
-	param set FW_T_SINK_MIN 4.0
-	param set FW_Y_ROLLFF 1.1
-	param set FW_L1_PERIOD 16
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
-fi
+sh /etc/init.d/rc.fw_defaults
  
-set VEHICLE_TYPE fw
-set MIXER FMU_RET
+set MIXER easystar
diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler
index 1ed923b19..dcc5db824 100644
--- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler
+++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler
@@ -1,40 +1,5 @@
 #!nsh
 
-echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
+sh /etc/init.d/rc.fw_defaults
 
-#
-# Load default params for this platform
-#
-if [ $DO_AUTOCONFIG == yes ]
-then
-	# Set all params here, then disable autoconfig
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 15
-	param set FW_P_LIM_MAX 50
-	param set FW_P_LIM_MIN -50
-	param set FW_P_P 60
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 1.1
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 100
-	param set FW_R_RMAX 100
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 1
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5.0
-	param set FW_T_SINK_MIN 4.0
-	param set FW_Y_ROLLFF 1.1
-	param set FW_L1_PERIOD 16
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
-	
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
-
-set VEHICLE_TYPE fw
 set MIXER FMU_AERT
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
index 1ed923b19..dcc5db824 100644
--- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
+++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
@@ -1,40 +1,5 @@
 #!nsh
 
-echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
+sh /etc/init.d/rc.fw_defaults
 
-#
-# Load default params for this platform
-#
-if [ $DO_AUTOCONFIG == yes ]
-then
-	# Set all params here, then disable autoconfig
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 15
-	param set FW_P_LIM_MAX 50
-	param set FW_P_LIM_MIN -50
-	param set FW_P_P 60
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 1.1
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 100
-	param set FW_R_RMAX 100
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 1
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5.0
-	param set FW_T_SINK_MIN 4.0
-	param set FW_Y_ROLLFF 1.1
-	param set FW_L1_PERIOD 16
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
-	
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
-
-set VEHICLE_TYPE fw
 set MIXER FMU_AERT
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800
new file mode 100644
index 000000000..9bc0262d8
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800
@@ -0,0 +1,5 @@
+#!nsh
+
+sh /etc/init.d/rc.fw_defaults
+
+set MIXER FMU_AET
diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
index cbcc6189b..83c470234 100644
--- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
@@ -1,40 +1,5 @@
 #!nsh
 
-echo "[init] PX4FMU v1, v2 with or without IO on Camflyer"
+sh /etc/init.d/rc.fw_defaults
 
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set FW_AIRSPD_MIN 7
-	param set FW_AIRSPD_TRIM 9
-	param set FW_AIRSPD_MAX 14
-	param set FW_L1_PERIOD 10
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 20
-	param set FW_P_LIM_MAX 30
-	param set FW_P_LIM_MIN -20
-	param set FW_P_P 30
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 2
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 60
-	param set FW_R_RMAX 60
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 0.7
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5
-	param set FW_T_SINK_MIN 2
-	param set FW_T_TIME_CONST 9
-	param set FW_Y_ROLLFF 2.0
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
-fi
-
-set VEHICLE_TYPE fw
 set MIXER FMU_Q
diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom
index 4ebbe9c61..d05c3174f 100644
--- a/ROMFS/px4fmu_common/init.d/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -2,43 +2,42 @@
 #
 # Phantom FPV Flying Wing
 #
-# Maintainers: Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
 #
 
+sh /etc/init.d/rc.fw_defaults
+
 if [ $DO_AUTOCONFIG == yes ]
 then
-	#
-	# Default parameters for this platform
-	#
-	param set FW_AIRSPD_MIN 11.4
-	param set FW_AIRSPD_TRIM 14
-	param set FW_AIRSPD_MAX 22
+	param set FW_AIRSPD_MIN 13
+	param set FW_AIRSPD_TRIM 15
+	param set FW_AIRSPD_MAX 25
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.75
 	param set FW_L1_PERIOD 15
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 15
-	param set FW_P_LIM_MAX 45
-	param set FW_P_LIM_MIN -45
-	param set FW_P_P 60
+	param set FW_PR_FF 0.2
+	param set FW_PR_I 0.005
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.03
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
 	param set FW_P_RMAX_NEG 0
 	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 2
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 15
-	param set FW_R_P 80
-	param set FW_R_RMAX 60
-	param set FW_THR_CRUISE 0.8
-	param set FW_THR_LND_MAX 0
-	param set FW_THR_MAX 1
-	param set FW_THR_MIN 0.5
-	param set FW_T_SINK_MAX 5.0
-	param set FW_T_SINK_MIN 2.0
-	param set FW_Y_ROLLFF 1.0
-	param set RC_SCALE_ROLL 0.6
-	param set RC_SCALE_PITCH 0.6
-	param set TRIM_PITCH 0.1
+	param set FW_P_ROLLFF 1
+	param set FW_RR_FF 0.5
+	param set FW_RR_I 0.02
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.08
+	param set FW_R_LIM 50
+	param set FW_R_RMAX 0
+	param set FW_T_HRATE_P 0.01
+	param set FW_T_RLL2THR 15
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 5
 fi
 
-set VEHICLE_TYPE fw
 set MIXER FMU_Q
+
+# Provide ESC a constant 1000 us pulse
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000
diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
index 143310af9..465166f25 100644
--- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
@@ -2,42 +2,38 @@
 #
 # Skywalker X5 Flying Wing
 #
-# Maintainers: Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
+# Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
 #
 
+sh /etc/init.d/rc.fw_defaults
+
 if [ $DO_AUTOCONFIG == yes ]
 then
-	#
-	# Default parameters for this platform
-	#
-	param set FW_AIRSPD_MIN 7
-	param set FW_AIRSPD_TRIM 9
-	param set FW_AIRSPD_MAX 14
-	param set FW_L1_PERIOD 10
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 20
-	param set FW_P_LIM_MAX 30
-	param set FW_P_LIM_MIN -20
-	param set FW_P_P 30
+	param set FW_AIRSPD_MIN 15
+	param set FW_AIRSPD_TRIM 20
+	param set FW_AIRSPD_MAX 40
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.74
+	param set FW_L1_PERIOD 15
+	param set FW_PR_FF 0.3
+	param set FW_PR_I 0
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.03
+	param set FW_P_LIM_MAX 45
+	param set FW_P_LIM_MIN -45
 	param set FW_P_RMAX_NEG 0
 	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 2
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 60
-	param set FW_R_RMAX 60
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 0.7
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5
-	param set FW_T_SINK_MIN 2
-	param set FW_T_TIME_CONST 9
-	param set FW_Y_ROLLFF 2.0
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
+	param set FW_P_ROLLFF 1
+	param set FW_RR_FF 0.3
+	param set FW_RR_I 0
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.03
+	param set FW_R_LIM 60
+	param set FW_R_RMAX 0
+	param set FW_T_HRATE_P 0.01
+	param set FW_T_RLL2THR 15
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 5
 fi
 
-set VEHICLE_TYPE fw
 set MIXER FMU_X5
diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing
index e53763278..f4dedef15 100644
--- a/ROMFS/px4fmu_common/init.d/3033_wingwing
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -2,42 +2,47 @@
 #
 # Wing Wing (aka Z-84) Flying Wing
 #
-# Maintainers: Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <sjwilks@gmail.com>
 #
 
+sh /etc/init.d/rc.fw_defaults
+
 if [ $DO_AUTOCONFIG == yes ]
 then
-	#
-	# Default parameters for this platform
-	#
-	param set FW_AIRSPD_MIN 7
-	param set FW_AIRSPD_TRIM 9
-	param set FW_AIRSPD_MAX 14
-	param set FW_L1_PERIOD 10
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 20
-	param set FW_P_LIM_MAX 30
-	param set FW_P_LIM_MIN -20
-	param set FW_P_P 30
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 2
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 60
-	param set FW_R_RMAX 60
-	param set FW_THR_CRUISE 0.65
-	param set FW_THR_MAX 0.7
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5
-	param set FW_T_SINK_MIN 2
-	param set FW_T_TIME_CONST 9
-	param set FW_Y_ROLLFF 2.0
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
+	param set BAT_N_CELLS 2
+	param set FW_AIRSPD_MAX 15
+	param set FW_AIRSPD_MIN 10
+	param set FW_AIRSPD_TRIM 13
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.74
+	param set FW_L1_PERIOD 16
+	param set FW_LND_ANG 15
+	param set FW_LND_FLALT 5
+	param set FW_LND_HHDIST 15
+	param set FW_LND_HVIRT 13
+	param set FW_LND_TLALT 5
+	param set FW_THR_LND_MAX 0
+	param set FW_PR_FF 0.35
+	param set FW_PR_I 0.005
+	param set FW_PR_IMAX 0.4
+	param set FW_PR_P 0.08
+	param set FW_RR_FF 0.6
+	param set FW_RR_I 0.005
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.04
+	param set MT_TKF_PIT_MAX 30.0
+	param set MT_ACC_D 0.2
+	param set MT_ACC_P 0.6
+	param set MT_A_LP 0.5
+	param set MT_PIT_OFF 0.1
+	param set MT_PIT_I 0.1
+	param set MT_THR_OFF 0.65
+	param set MT_THR_I 0.35
+	param set MT_THR_P  0.2
+	param set MT_THR_FF 1.5
 fi
 
-set VEHICLE_TYPE fw
-set MIXER FMU_Q
+set MIXER wingwing
+# Provide ESC a constant 1000 us pulse
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000
diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79
index 8d179d1fd..f4bd18269 100644
--- a/ROMFS/px4fmu_common/init.d/3034_fx79
+++ b/ROMFS/px4fmu_common/init.d/3034_fx79
@@ -2,42 +2,9 @@
 #
 # FX-79 Buffalo Flying Wing
 #
-# Maintainers: Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <sjwilks@gmail.com>
 #
 
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set FW_AIRSPD_MAX 20
-	param set FW_AIRSPD_TRIM 12
-	param set FW_AIRSPD_MIN 15
-	param set FW_L1_PERIOD 12
-	param set FW_P_D 0
-	param set FW_P_I 0
-	param set FW_P_IMAX 15
-	param set FW_P_LIM_MAX 50
-	param set FW_P_LIM_MIN -50
-	param set FW_P_P 60
-	param set FW_P_RMAX_NEG 0
-	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 1.1
-	param set FW_R_D 0
-	param set FW_R_I 5
-	param set FW_R_IMAX 20
-	param set FW_R_P 80
-	param set FW_R_RMAX 100
-	param set FW_THR_CRUISE 0.75
-	param set FW_THR_MAX 1
-	param set FW_THR_MIN 0
-	param set FW_T_SINK_MAX 5.0
-	param set FW_T_SINK_MIN 4.0
-	param set FW_T_TIME_CONST 9
-	param set FW_Y_ROLLFF 1.1	
-	param set RC_SCALE_ROLL 1.0
-	param set RC_SCALE_PITCH 1.0
-fi
+sh /etc/init.d/rc.fw_defaults
 
-set VEHICLE_TYPE fw
 set MIXER FMU_FX79
diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
new file mode 100644
index 000000000..7dbda54d3
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
@@ -0,0 +1,42 @@
+#!nsh
+#
+# TBS Caipirinha Flying Wing
+#
+# Thomas Gubler <thomasgubler@gmail.com>
+#
+
+sh /etc/init.d/rc.fw_defaults
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	
+	# TODO: these are the X5 default parameters, update them to the caipi
+
+	param set FW_AIRSPD_MIN 15
+	param set FW_AIRSPD_TRIM 20
+	param set FW_AIRSPD_MAX 40
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.74
+	param set FW_L1_PERIOD 15
+	param set FW_PR_FF 0.3
+	param set FW_PR_I 0
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.03
+	param set FW_P_LIM_MAX 45
+	param set FW_P_LIM_MIN -45
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 0
+	param set FW_RR_FF 0.3
+	param set FW_RR_I 0
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.03
+	param set FW_R_LIM 60
+	param set FW_R_RMAX 0
+	param set FW_T_HRATE_P 0.01
+	param set FW_T_RLL2THR 15
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 5
+fi
+
+set MIXER FMU_Q
diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x
new file mode 100644
index 000000000..06c54a41d
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4001_quad_x
@@ -0,0 +1,12 @@
+#!nsh
+#
+# Generic 10" Quad X geometry
+#
+# Lorenz Meier <lm@inf.ethz.ch>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER FMU_quad_x
+
+set PWM_OUTPUTS 1234
diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone
index f6f09ac22..e6007db0e 100644
--- a/ROMFS/px4fmu_common/init.d/4008_ardrone
+++ b/ROMFS/px4fmu_common/init.d/4008_ardrone
@@ -1,57 +1,38 @@
 #!nsh
-
-echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board"
- 
 #
-# Load default params for this platform
+# ARDrone
 #
-if param compare SYS_AUTOCONFIG 1
-then
-	# Set all params here, then disable autoconfig
-	param set MC_ATTRATE_D 0
-	param set MC_ATTRATE_I 0
-	param set MC_ATTRATE_P 0.13
-	param set MC_ATT_D 0
-	param set MC_ATT_I 0.3
-	param set MC_ATT_P 5
-	param set MC_YAWPOS_D 0.1
-	param set MC_YAWPOS_I 0.15
-	param set MC_YAWPOS_P 1
-	param set MC_YAWRATE_D 0
-	param set MC_YAWRATE_I 0
-	param set MC_YAWRATE_P 0.15
 
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
+echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board"
 
-#
-# Force some key parameters to sane values
-# MAV_TYPE     2 = quadrotor
-#
-param set MAV_TYPE 2
-param set BAT_V_SCALING 0.008381
+# Just use the default multicopter settings.
+sh /etc/init.d/rc.mc_defaults
 
 #
-# Start MAVLink
+# Load default params for this platform
 #
-mavlink start -d /dev/ttyS0 -b 57600
-usleep 5000
+if [ $DO_AUTOCONFIG == yes ]
+then
+    # Set all params here, then disable autoconfig
+    param set MC_ROLL_P 6.0
+    param set MC_ROLLRATE_P 0.14
+    param set MC_ROLLRATE_I 0.1
+    param set MC_ROLLRATE_D 0.002
+    param set MC_PITCH_P 6.0
+    param set MC_PITCHRATE_P 0.14
+    param set MC_PITCHRATE_I 0.1
+    param set MC_PITCHRATE_D 0.002
+    param set MC_YAW_P 2.0
+    param set MC_YAWRATE_P 0.2
+    param set MC_YAWRATE_I 0.2
+    param set MC_YAWRATE_D 0.0
+    param set MC_YAW_FF 0.8
+    
+    param set BAT_V_SCALING 0.00838095238
+fi
 
-#
-# Configure PX4FMU for operation with PX4IOAR
-#
-fmu mode_gpio_serial
- 
-#
-# Fire up the AR.Drone interface.
-#
-ardrone_interface start -d /dev/ttyS1
- 
-#
-# Start common for all multirotors apps
-#
-sh /etc/init.d/rc.multirotor
- 
-# Exit, because /dev/ttyS0 is needed for MAVLink
-exit
+set OUTPUT_MODE ardrone
+set USE_IO no
+set MIXER skip
+# set MAV_TYPE because no specific mixer is set
+set MAV_TYPE 2
diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow
deleted file mode 100644
index 2886bcb75..000000000
--- a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow
+++ /dev/null
@@ -1,83 +0,0 @@
-#!nsh
-
-echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW"
-
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
-then
-	# Set all params here, then disable autoconfig
-	param set MC_ATTRATE_D 0
-	param set MC_ATTRATE_I 0
-	param set MC_ATTRATE_P 0.13
-	param set MC_ATT_D 0
-	param set MC_ATT_I 0.3
-	param set MC_ATT_P 5
-	param set MC_YAWPOS_D 0.1
-	param set MC_YAWPOS_I 0.15
-	param set MC_YAWPOS_P 1
-	param set MC_YAWRATE_D 0
-	param set MC_YAWRATE_I 0
-	param set MC_YAWRATE_P 0.15
-
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE     2 = quadrotor
-#
-param set MAV_TYPE 2
-param set BAT_V_SCALING 0.008381
-
-#
-# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor)
-#
-mavlink start -d /dev/ttyS0 -b 57600
-mavlink_onboard start -d /dev/ttyS3 -b 115200
-usleep 5000
-
-#
-# Configure PX4FMU for operation with PX4IOAR
-#
-fmu mode_gpio_serial
-
-#
-# Fire up the AR.Drone interface.
-#
-ardrone_interface start -d /dev/ttyS1
- 
-#
-# Start the sensors.
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start the attitude estimator
-#
-attitude_estimator_ekf start
-
-#
-# Start the position estimator
-#
-flow_position_estimator start
-
-#
-# Fire up the multi rotor attitude controller
-#
-multirotor_att_control start
-
-#
-# Fire up the flow position controller
-#
-flow_position_control start
-
-#
-# Fire up the flow speed controller
-#
-flow_speed_control start
- 
-# Exit, because /dev/ttyS0 is needed for MAVLink
-exit
diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330
index ab1db94d0..e6e2e19dc 100644
--- a/ROMFS/px4fmu_common/init.d/4010_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -2,50 +2,26 @@
 #
 # DJI Flame Wheel F330 Quadcopter
 #
-# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton.babushkin@me.com>
 #
 
+sh /etc/init.d/4001_quad_x
+
 if [ $DO_AUTOCONFIG == yes ]
 then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.8
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.05
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
-	
-	param set MPC_TILT_MAX 0.5
-	param set MPC_THR_MAX 0.8
-	param set MPC_THR_MIN 0.2
-	param set MPC_XY_D 0
-	param set MPC_XY_P 0.5
-	param set MPC_XY_VEL_D 0
-	param set MPC_XY_VEL_I 0
-	param set MPC_XY_VEL_MAX 3
-	param set MPC_XY_VEL_P 0.2
-	param set MPC_Z_D 0
-	param set MPC_Z_P 1
-	param set MPC_Z_VEL_D 0
-	param set MPC_Z_VEL_I 0.1
-	param set MPC_Z_VEL_MAX 2
-	param set MPC_Z_VEL_P 0.20
 fi
 
-set VEHICLE_TYPE mc
-set MIXER FMU_quad_x
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
+set PWM_MIN 1175
 set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
index 299771c1d..3465b59cf 100644
--- a/ROMFS/px4fmu_common/init.d/4011_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -2,36 +2,27 @@
 #
 # DJI Flame Wheel F450 Quadcopter
 #
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lm@inf.ethz.ch>
 #
 
+sh /etc/init.d/4001_quad_x
+
 if [ $DO_AUTOCONFIG == yes ]
 then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
+	# TODO REVIEW
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
 fi
 
-set VEHICLE_TYPE mc
-set MIXER FMU_quad_x
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
+set PWM_MIN 1175
 set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550
deleted file mode 100644
index 7e020cf59..000000000
--- a/ROMFS/px4fmu_common/init.d/4012_hk_x550
+++ /dev/null
@@ -1,33 +0,0 @@
-#!nsh
-#
-# HobbyKing X550 Quadcopter
-#
-# Maintainers: Todd Stellanova <tstellanova@gmail.com>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 5.5
-	param set MC_ATT_I 0
-	param set MC_ATT_D 0
-	param set MC_ATTRATE_P 0.14
-	param set MC_ATTRATE_I 0
-	param set MC_ATTRATE_D 0.006
-	param set MC_YAWPOS_P 0.6
-	param set MC_YAWPOS_I 0
-	param set MC_YAWPOS_D 0
-	param set MC_YAWRATE_P 0.08
-	param set MC_YAWRATE_I 0
-	param set MC_YAWRATE_D 0
-
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_quad_x
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
new file mode 100644
index 000000000..99ffd73a5
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
@@ -0,0 +1,28 @@
+#!nsh
+#
+# Hobbyking Micro Integrated PCB Quadcopter
+# with SimonK ESC firmware and Mystery A1510 motors
+#
+# Thomas Gubler <thomasgubler@gmail.com>
+#
+echo "HK Micro PCB Quad"
+
+sh /etc/init.d/4001_quad_x
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_D 0.0
+fi
+
+set PWM_MIN 1200
diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+
new file mode 100644
index 000000000..1fb25e5d8
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/5001_quad_+
@@ -0,0 +1,12 @@
+#!nsh
+#
+# Generic 10" Quad + geometry
+#
+# Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER FMU_quad_+
+
+set PWM_OUTPUTS 1234
diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm
deleted file mode 100644
index 2e5f6ca4f..000000000
--- a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm
+++ /dev/null
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Quad + geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_quad_+
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x
new file mode 100644
index 000000000..34fc6523f
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x
@@ -0,0 +1,13 @@
+#!nsh
+#
+# Generic 10" Hexa X geometry
+#
+# Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER FMU_hexa_x
+
+# 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/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
deleted file mode 100644
index 447796709..000000000
--- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
+++ /dev/null
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Hexa X geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_hexa_x
-
-set PWM_OUTPUTS 12345678
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+
new file mode 100644
index 000000000..235e376a6
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+
@@ -0,0 +1,13 @@
+#!nsh
+#
+# Generic 10" Hexa + geometry
+#
+# Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER FMU_hexa_+
+
+# 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/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
deleted file mode 100644
index c4e9560d1..000000000
--- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
+++ /dev/null
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Hexa + geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_hexa_+
-
-set PWM_OUTPUTS 12345678
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x
new file mode 100644
index 000000000..769217dc7
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/8001_octo_x
@@ -0,0 +1,12 @@
+#!nsh
+#
+# Generic 10" Octo X geometry
+#
+# Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER FMU_octo_x
+
+set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
deleted file mode 100644
index ea56195d4..000000000
--- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
+++ /dev/null
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Octo X geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_octo_x
-
-set PWM_OUTPUTS 12345678
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+
new file mode 100644
index 000000000..28b074a54
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/9001_octo_+
@@ -0,0 +1,12 @@
+#!nsh
+#
+# Generic 10" Octo + geometry
+#
+# Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER FMU_octo_+
+
+set PWM_OUTPUTS 12345678
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
deleted file mode 100644
index f7693875c..000000000
--- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
+++ /dev/null
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Octo + geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_octo_+
-
-set PWM_OUTPUTS 12345678
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 38b1cb57e..5d9e9502c 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -31,11 +31,6 @@ then
 	sh /etc/init.d/1001_rc_quad_x.hil
 fi
 
-if param compare SYS_AUTOSTART 1002
-then
-	sh /etc/init.d/1002_rc_fw_state.hil
-fi
-
 if param compare SYS_AUTOSTART 1003
 then
 	sh /etc/init.d/1003_rc_quad_+.hil
@@ -46,6 +41,11 @@ then
 	sh /etc/init.d/1004_rc_fw_Rascal110.hil
 fi
 
+if param compare SYS_AUTOSTART 1005
+then
+	sh /etc/init.d/1005_rc_fw_Malolo1.hil
+fi
+
 #
 # Standard plane
 #
@@ -68,6 +68,12 @@ then
 	set MODE custom
 fi
 
+if param compare SYS_AUTOSTART 2103 103
+then
+        sh /etc/init.d/2103_skyhunter_1800
+        set MODE custom
+fi
+
 #
 # Flying wing
 #
@@ -97,18 +103,27 @@ then
 	sh /etc/init.d/3034_fx79
 fi
 
+if param compare SYS_AUTOSTART 3100
+then
+	sh /etc/init.d/3100_tbs_caipirinha
+fi
+
 #
 # Quad X
 #
 
-if param compare SYS_AUTOSTART 4008 8
+if param compare SYS_AUTOSTART 4001
 then
-	#sh /etc/init.d/4008_ardrone
+	sh /etc/init.d/4001_quad_x
 fi
 
-if param compare SYS_AUTOSTART 4009 9
+#
+# ARDrone
+#
+
+if param compare SYS_AUTOSTART 4008 8
 then
-	#sh /etc/init.d/4009_ardrone_flow
+    sh /etc/init.d/4008_ardrone
 fi
 
 if param compare SYS_AUTOSTART 4010 10
@@ -121,9 +136,9 @@ then
 	sh /etc/init.d/4011_dji_f450
 fi
 
-if param compare SYS_AUTOSTART 4012 12
+if param compare SYS_AUTOSTART 4020
 then
-	sh /etc/init.d/4012_hk_x550
+	sh /etc/init.d/4020_hk_micro_pcb
 fi
 
 #
@@ -132,7 +147,7 @@ fi
 
 if param compare SYS_AUTOSTART 5001
 then
-	sh /etc/init.d/5001_quad_+_pwm
+	sh /etc/init.d/5001_quad_+
 fi
 
 #
@@ -141,7 +156,7 @@ fi
 
 if param compare SYS_AUTOSTART 6001
 then
-	sh /etc/init.d/6001_hexa_x_pwm
+	sh /etc/init.d/6001_hexa_x
 fi
 
 #
@@ -150,7 +165,7 @@ fi
 
 if param compare SYS_AUTOSTART 7001
 then
-	sh /etc/init.d/7001_hexa_+_pwm
+	sh /etc/init.d/7001_hexa_+
 fi
 
 #
@@ -159,7 +174,7 @@ fi
 
 if param compare SYS_AUTOSTART 8001
 then
-	sh /etc/init.d/8001_octo_x_pwm
+	sh /etc/init.d/8001_octo_x
 fi
 
 #
@@ -168,7 +183,7 @@ fi
 
 if param compare SYS_AUTOSTART 9001
 then
-	sh /etc/init.d/9001_octo_+_pwm
+	sh /etc/init.d/9001_octo_+
 fi
 
 #
@@ -185,6 +200,11 @@ then
 	sh /etc/init.d/10016_3dr_iris
 fi
 
+if param compare SYS_AUTOSTART 10017
+then
+	sh /etc/init.d/10017_steadidrone_qu4d
+fi
+
 #
 # Hexa Coaxial
 #
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps
index d354fb06f..9aca3fc5f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps
@@ -6,14 +6,10 @@
 #
 # Start the attitude and position estimator
 #
-att_pos_estimator_ekf start
+ekf_att_pos_estimator start
 
 #
 # Start attitude controller
 #
 fw_att_control start
-
-#
-# Start the position controller
-#
 fw_pos_control_l1 start
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
new file mode 100644
index 000000000..3a50fcf56
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -0,0 +1,14 @@
+#!nsh
+
+set VEHICLE_TYPE fw
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+#
+# Default parameters for FW
+#
+	param set NAV_LAND_ALT 90
+	param set NAV_RTL_ALT 100
+	param set NAV_RTL_LAND_T -1
+	param set NAV_ACCEPT_RAD 50
+fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index d25f01dde..7f793b219 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -3,7 +3,7 @@
 # Script to configure control interface
 #
 
-if [ $MIXER != none ]
+if [ $MIXER != none -a $MIXER != skip ]
 then
 	#
 	# Load mixer
@@ -33,8 +33,11 @@ then
 		tone_alarm $TUNE_OUT_ERROR
 	fi
 else
-	echo "[init] Mixer not defined"
-	tone_alarm $TUNE_OUT_ERROR
+	if [ $MIXER != skip ]
+	then
+		echo "[init] Mixer not defined"
+		tone_alarm $TUNE_OUT_ERROR
+	fi
 fi
 
 if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
index c9d964f8e..e23aebd87 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -4,7 +4,6 @@
 
 #
 # Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
 #
 px4io recovery
 
@@ -12,7 +11,7 @@ px4io recovery
 # Adjust PX4IO update rate limit
 #
 set PX4IO_LIMIT 400
-if hw_ver compare PX4FMU_V1
+if ver hwcmp PX4FMU_V1
 then
 	set PX4IO_LIMIT 200
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index ac620844c..25f31a7e0 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -5,10 +5,12 @@
 
 if [ -d /fs/microsd ]
 then
-	if hw_ver compare PX4FMU_V1
+	if ver hwcmp PX4FMU_V1
 	then
-		sdlog2 start -r 50 -a -b 16 -t
+		echo "Start sdlog2 at 50Hz"
+		sdlog2 start -r 50 -a -b 4 -t
 	else
-		sdlog2 start -r 200 -a -b 16 -t
+		echo "Start sdlog2 at 200Hz"
+		sdlog2 start -r 200 -a -b 22 -t
 	fi
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 8b51d57e5..268eb9bba 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -1,24 +1,12 @@
 #!nsh
 #
-# Standard apps for multirotors
+# Standard apps for multirotors:
+# att & pos estimator, att & pos control.
 #
 
-#
-# Start the attitude estimator
-#
 attitude_estimator_ekf start
- 
-#
-# Start position estimator
-#
+#ekf_att_pos_estimator start
 position_estimator_inav start
 
-#
-# Start attitude control
-#
-multirotor_att_control start
-
-#
-# Start position control
-#
-multirotor_pos_control start
+mc_att_control start
+mc_pos_control start
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
new file mode 100644
index 000000000..0df320f49
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
@@ -0,0 +1,50 @@
+#!nsh
+
+set VEHICLE_TYPE mc
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.5
+
+	param set MPC_THR_MAX 1.0
+	param set MPC_THR_MIN 0.1
+	param set MPC_XY_P 1.0
+	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
+	param set MPC_Z_P 1.0
+	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILTMAX_AIR 45.0
+	param set MPC_TILTMAX_LND 15.0
+	param set MPC_LAND_SPEED 1.0
+
+	param set PE_VELNE_NOISE 0.5
+	param set PE_VELD_NOISE 0.7
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.0
+
+	param set NAV_ACCEPT_RAD 2.0
+fi
+
+set PWM_RATE 400
+set PWM_DISARMED 900
+set PWM_MIN 1075
+set PWM_MAX 2000
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index badbf92c3..be54ea98b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -3,10 +3,6 @@
 # Standard startup script for PX4FMU onboard sensor drivers.
 #
 
-#
-# Start sensor drivers here.
-#
-
 ms5611 start
 adc start
 
@@ -26,8 +22,9 @@ then
 	echo "[init] Using L3GD20(H)"
 fi
 
-if hw_ver compare PX4FMU_V2
+if ver hwcmp PX4FMU_V2
 then
+	# IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST
 	if lsm303d start
 	then
 		echo "[init] Using LSM303D"
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 0cd8a0e04..1f8d8b862 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -3,40 +3,22 @@
 # USB MAVLink start
 #
 
-echo "Starting MAVLink on this USB console"
-
-# Stop tone alarm
-tone_alarm stop
-
-#
-# Check for UORB
-#
-if uorb start
-then
-	echo "uORB started"
-fi
-
-# Tell MAVLink that this link is "fast"
-if mavlink stop
-then
-	echo "stopped other MAVLink instance"
-fi
-mavlink start -b 230400 -d /dev/ttyACM0
-
-# Stop commander
-if commander stop
-then
-	echo "Commander stopped"
-fi
-sleep 1
-
-# Start the commander
-if commander start
-then
-	echo "Commander started"
-fi
-
-echo "MAVLink started, exiting shell.."
+mavlink start -r 10000 -d /dev/ttyACM0 -x
+# Enable a number of interesting streams we want via USB
+mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
+usleep 100000
 
 # Exit shell to make it available to MAVLink
-exit 
+exit
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index d12785368..8855539fe 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -3,8 +3,7 @@
 # PX4FMU startup script.
 
 #
-# Default to auto-start mode.  An init script on the microSD card
-# can change this to prevent automatic startup of the flight script.
+# Default to auto-start mode.
 #
 set MODE autostart
 
@@ -21,7 +20,7 @@ echo "[init] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	set LOG_FILE /fs/microsd/bootlog.txt
-	echo "[init] microSD card mounted at /fs/microsd"
+	echo "[init] microSD mounted: /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
@@ -66,12 +65,12 @@ then
 	# Start CDC/ACM serial driver
 	#
 	sercon
-	
+
 	#
 	# Start the ORB (first app to start)
 	#
 	uorb start
-	
+
 	#
 	# Load parameters
 	#
@@ -80,36 +79,39 @@ then
 	then
 		set PARAM_FILE /fs/mtd_params
 	fi
-	
+
 	param select $PARAM_FILE
 	if param load
 	then
-		echo "[init] Parameters loaded: $PARAM_FILE"
+		echo "[init] Params loaded: $PARAM_FILE"
 	else
-		echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
+		echo "[init] ERROR: Params loading failed: $PARAM_FILE"
 	fi
-	
+
 	#
 	# Start system state indicator
 	#
 	if rgbled start
 	then
-		echo "[init] Using external RGB Led"
+		echo "[init] RGB Led"
 	else
 		if blinkm start
 		then
-			echo "[init] Using blinkm"
+			echo "[init] BlinkM"
 			blinkm systemstate
 		fi
 	fi
-	
+
+	if pca8574 start
+	then
+	fi
+
 	#
 	# Set default values
 	#
 	set HIL no
 	set VEHICLE_TYPE none
 	set MIXER none
-	set USE_IO yes
 	set OUTPUT_MODE none
 	set PWM_OUTPUTS none
 	set PWM_RATE none
@@ -118,24 +120,43 @@ then
 	set PWM_MAX none
 	set MKBLCTRL_MODE none
 	set FMU_MODE pwm
+	set MAVLINK_FLAGS default
+	set EXIT_ON_END no
 	set MAV_TYPE none
-	
+	set LOAD_DEFAULT_APPS yes
+	set GPS yes
+	set GPS_FAKE no
+
 	#
 	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
 	#
 	if param compare SYS_AUTOCONFIG 1
 	then
+		# We can't be sure the defaults haven't changed, so
+		# if someone requests a re-configuration, we do it
+		# cleanly from scratch (except autostart / autoconfig)
+		param reset_nostart
 		set DO_AUTOCONFIG yes
 	else
 		set DO_AUTOCONFIG no
 	fi
-		
+
+	#
+	# Set USE_IO flag
+	#
+	if param compare SYS_USE_IO 1
+	then
+		set USE_IO yes
+	else
+		set USE_IO no
+	fi
+
 	#
 	# Set parameters and env variables for selected AUTOSTART
 	#
 	if param compare SYS_AUTOSTART 0
 	then
-		echo "[init] Don't try to find autostart script"
+		echo "[init] No autostart"
 	else
 		sh /etc/init.d/rc.autostart
 	fi
@@ -145,10 +166,10 @@ then
 	#
 	if [ -f $CONFIG_FILE ]
 	then
-		echo "[init] Reading config: $CONFIG_FILE"
+		echo "[init] Config: $CONFIG_FILE"
 		sh $CONFIG_FILE
 	else
-		echo "[init] Config file not found: $CONFIG_FILE"
+		echo "[init] Config not found: $CONFIG_FILE"
 	fi
 
 	#
@@ -159,9 +180,9 @@ then
 		param set SYS_AUTOCONFIG 0
 		param save
 	fi
-	
+
 	set IO_PRESENT no
-	
+
 	if [ $USE_IO == yes ]
 	then
 		#
@@ -173,19 +194,19 @@ then
 		else
 			set IO_FILE /etc/extras/px4io-v1_default.bin
 		fi
-		
+
 		if px4io checkcrc $IO_FILE
 		then
 			echo "[init] PX4IO CRC OK"
 			echo "PX4IO CRC OK" >> $LOG_FILE
-	
+
 			set IO_PRESENT yes
 		else
 			echo "[init] Trying to update"
 			echo "PX4IO Trying to update" >> $LOG_FILE
-			
+
 			tone_alarm MLL32CP8MB
-			
+
 			if px4io forceupdate 14662 $IO_FILE
 			then
 				usleep 500000
@@ -194,7 +215,7 @@ then
 					echo "[init] PX4IO CRC OK, update successful"
 					echo "PX4IO CRC OK after updating" >> $LOG_FILE
 					tone_alarm MLL8CDE
-	
+
 					set IO_PRESENT yes
 				else
 					echo "[init] ERROR: PX4IO update failed"
@@ -207,14 +228,14 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		
+
 		if [ $IO_PRESENT == no ]
 		then
 			echo "[init] ERROR: PX4IO not found"
 			tone_alarm $TUNE_OUT_ERROR
 		fi
 	fi
-	
+
 	#
 	# Set default output if not set
 	#
@@ -233,36 +254,46 @@ then
 		# Need IO for output but it not present, disable output
 		set OUTPUT_MODE none
 		echo "[init] ERROR: PX4IO not found, disabling output"
-		
+
 		# Avoid using ttyS0 for MAVLink on FMUv1
-		if hw_ver compare PX4FMU_V1
+		if ver hwcmp PX4FMU_V1
 		then
 			set FMU_MODE serial
 		fi
 	fi
 
+	if [ $OUTPUT_MODE == ardrone ]
+	then
+		set FMU_MODE gpio_serial
+	fi
+
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil
-		if hw_ver compare PX4FMU_V1
+		if ver hwcmp PX4FMU_V1
 		then
 			set FMU_MODE serial
 		fi
-	else
-		# Try to get an USB console if not in HIL mode
-		nshterm /dev/ttyACM0 &
 	fi
-	
+
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+
+	#
+	# Start the datamanager
+	#
+	dataman start
+
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)
 	#
 	commander start
-	
+
 	#
 	# Start primary output
 	#
 	set TTYS1_BUSY no
-	
+
 	# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
 	if [ $OUTPUT_MODE != none ]
 	then
@@ -278,9 +309,10 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		if [ $OUTPUT_MODE == fmu ]
+
+		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
 		then
-			echo "[init] Use FMU PWM as primary output"
+			echo "[init] Use FMU as primary output"
 			if fmu mode_$FMU_MODE
 			then
 				echo "[init] FMU mode_$FMU_MODE started"
@@ -288,19 +320,20 @@ then
 				echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
 				tone_alarm $TUNE_OUT_ERROR
 			fi
-			
-			if hw_ver compare PX4FMU_V1
+
+			if ver hwcmp PX4FMU_V1
 			then
 				if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
 				then
 					set TTYS1_BUSY yes
 				fi
-				if [ $FMU_MODE == pwm_gpio ]
+				if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
 				then
 					set TTYS1_BUSY yes
 				fi
 			fi
 		fi
+
 		if [ $OUTPUT_MODE == mkblctrl ]
 		then
 			echo "[init] Use MKBLCTRL as primary output"
@@ -313,7 +346,7 @@ then
 			then
 				set MKBLCTRL_ARG "-mkmode +"
 			fi
-			
+
 			if mkblctrl $MKBLCTRL_ARG
 			then
 				echo "[init] MKBLCTRL started"
@@ -321,12 +354,13 @@ then
 				echo "[init] ERROR: MKBLCTRL start failed"
 				tone_alarm $TUNE_OUT_ERROR
 			fi
-			
-		fi		
+
+		fi
+
 		if [ $OUTPUT_MODE == hil ]
 		then
 			echo "[init] Use HIL as primary output"
-			if hil mode_pwm
+			if hil mode_port2_pwm8
 			then
 				echo "[init] HIL output started"
 			else
@@ -334,7 +368,7 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		
+
 		#
 		# Start IO or FMU for RC PPM input if needed
 		#
@@ -352,7 +386,7 @@ then
 				fi
 			fi
 		else
-			if [ $OUTPUT_MODE != fmu ]
+			if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
 			then
 				if fmu mode_$FMU_MODE
 				then
@@ -361,14 +395,14 @@ then
 					echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
 					tone_alarm $TUNE_OUT_ERROR
 				fi
-	
-				if hw_ver compare PX4FMU_V1
+
+				if ver hwcmp PX4FMU_V1
 				then
 					if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
 					then
 						set TTYS1_BUSY yes
 					fi
-					if [ $FMU_MODE == pwm_gpio ]
+					if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
 					then
 						set TTYS1_BUSY yes
 					fi
@@ -376,74 +410,87 @@ then
 			fi
 		fi
 	fi
-	
+
 	#
 	# MAVLink
 	#
-	set EXIT_ON_END no
-	
-	if [ $HIL == yes ]
+	if [ $MAVLINK_FLAGS == default ]
 	then
-		sleep 1
-		mavlink start -b 230400 -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
-			mavlink start -d /dev/ttyS0
-			usleep 5000
-			
+			set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
+
 			# Exit from nsh to free port for mavlink
 			set EXIT_ON_END yes
 		else
 			# Start MAVLink on default port: ttyS1
-			mavlink start
-			usleep 5000
+			set MAVLINK_FLAGS "-r 1000"
 		fi
 	fi
-	
+
+	mavlink start $MAVLINK_FLAGS
+
 	#
 	# Sensors, Logging, GPS
 	#
-	echo "[init] Start sensors"
 	sh /etc/init.d/rc.sensors
 
-	if [ $HIL == no ]
+	#
+	# Start logging in all modes, including HIL
+	#
+	sh /etc/init.d/rc.logging
+
+	if [ $GPS == yes ]
 	then
-		echo "[init] Start logging"
-		sh /etc/init.d/rc.logging
-		
 		echo "[init] Start GPS"
-		gps start
+		if [ $GPS_FAKE == yes ]
+		then
+			echo "[init] Faking GPS"
+			gps start -f
+		else
+			gps start
+		fi
 	fi
-	
+
+	#
+	# Start up ARDrone Motor interface
+	#
+	if [ $OUTPUT_MODE == ardrone ]
+	then
+		ardrone_interface start -d /dev/ttyS1
+	fi
+
 	#
 	# Fixed wing setup
 	#
 	if [ $VEHICLE_TYPE == fw ]
 	then
 		echo "[init] Vehicle type: FIXED WING"
-		
+
 		if [ $MIXER == none ]
 		then
 			# Set default mixer for fixed wing if not defined
 			set MIXER FMU_AERT
 		fi
-		
+
 		if [ $MAV_TYPE == none ]
 		then
 			# Use MAV_TYPE = 1 (fixed wing) if not defined
 			set MAV_TYPE 1
 		fi
-		
+
 		param set MAV_TYPE $MAV_TYPE
-		
+
 		# Load mixer and configure outputs
 		sh /etc/init.d/rc.interface
-		
+
 		# Start standard fixedwing apps
-		sh /etc/init.d/rc.fw_apps
+		if [ $LOAD_DEFAULT_APPS == yes ]
+		then
+			sh /etc/init.d/rc.fw_apps
+		fi
 	fi
 
 	#
@@ -455,21 +502,25 @@ then
 
 		if [ $MIXER == none ]
 		then
-			# Set default mixer for multicopter if not defined
-			set MIXER quad_x
+			echo "Default mixer for multicopter not defined"
 		fi
 
 		if [ $MAV_TYPE == none ]
 		then
-			# Use MAV_TYPE = 2 (quadcopter) if not defined
-			set MAV_TYPE 2
-			
 			# Use mixer to detect vehicle type
+			if [ $MIXER == FMU_quad_x -o $MIXER == FMU_quad_+ ]
+			then
+				set MAV_TYPE 2
+			fi
+			if [ $MIXER == FMU_quad_w ]
+			then
+				set MAV_TYPE 2
+			fi
 			if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ]
 			then
 				set MAV_TYPE 13
 			fi
-			if [ $MIXER == hexa_cox ]
+			if [ $MIXER == FMU_hexa_cox ]
 			then
 				set MAV_TYPE 13
 			fi
@@ -483,24 +534,36 @@ then
 			fi
 		fi
 
-		param set MAV_TYPE $MAV_TYPE
-		
+		# Still no MAV_TYPE found
+		if [ $MAV_TYPE == none ]
+		then
+			echo "Unknown MAV_TYPE"
+		else
+			param set MAV_TYPE $MAV_TYPE
+		fi
+
 		# Load mixer and configure outputs
 		sh /etc/init.d/rc.interface
-		
+
 		# Start standard multicopter apps
-		sh /etc/init.d/rc.mc_apps
+		if [ $LOAD_DEFAULT_APPS == yes ]
+		then
+			sh /etc/init.d/rc.mc_apps
+		fi
 	fi
 
 	#
+	# Start the navigator
+	#
+	navigator start
+
+	#
 	# Generic setup (autostart ID not found)
 	#
 	if [ $VEHICLE_TYPE == none ]
 	then
-		echo "[init] Vehicle type: GENERIC"
+		echo "[init] Vehicle type: No autostart ID found"
 
-		# Load mixer and configure outputs
-		sh /etc/init.d/rc.interface
 	fi
 
 	# Start any custom addons
@@ -509,11 +572,12 @@ then
 		echo "[init] Starting addons script: $EXTRAS_FILE"
 		sh $EXTRAS_FILE
 	else
-		echo "[init] Addons script not found: $EXTRAS_FILE"
+		echo "[init] No addons script: $EXTRAS_FILE"
 	fi
 
 	if [ $EXIT_ON_END == yes ]
 	then
+		echo "[init] Exit from nsh"
 		exit
 	fi
 
diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix
index 497786feb..497786feb 100644
--- a/ROMFS/px4fmu_common/mixers/hexa_cox.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix
diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix
index 2cb70e814..397e22086 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix
@@ -1,7 +1,3 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a octocopter in the + configuration. All controls
-are mixed 100%.
+# Octo +
 
 R: 8+ 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README
new file mode 100644
index 000000000..60311232e
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/README
@@ -0,0 +1,154 @@
+PX4 mixer definitions
+=====================
+
+Files in this directory implement example mixers that can be used as a basis
+for customisation, or for general testing purposes.
+
+Mixer basics
+------------
+
+Mixers combine control values from various sources (control tasks, user inputs,
+etc.) and produce output values suitable for controlling actuators; servos,
+motors, switches and so on.
+
+An actuator derives its value from the combination of one or more control
+values. Each of the control values is scaled according to the actuator's
+configuration and then combined to produce the actuator value, which may then be
+further scaled to suit the specific output type.
+
+Internally, all scaling is performed using floating point values. Inputs and
+outputs are clamped to the range -1.0 to 1.0.
+
+control    control   control
+   |          |         |
+   v          v         v
+ scale      scale     scale
+   |          |         |
+   |          v         |
+   +-------> mix <------+
+              |
+            scale
+              |
+              v
+             out
+
+Scaling
+-------
+
+Basic scalers provide linear scaling of the input to the output.
+
+Each scaler allows the input value to be scaled independently for inputs
+greater/less than zero. An offset can be applied to the output, and lower and
+upper boundary constraints can be applied. Negative scaling factors cause the
+output to be inverted (negative input produces positive output).
+
+Scaler pseudocode:
+
+if (input < 0)
+    output = (input * NEGATIVE_SCALE) + OFFSET
+else
+    output = (input * POSITIVE_SCALE) + OFFSET
+
+if (output < LOWER_LIMIT)
+    output = LOWER_LIMIT
+if (output > UPPER_LIMIT)
+    output = UPPER_LIMIT
+
+Syntax
+------
+
+Mixer definitions are text files; lines beginning with a single capital letter
+followed by a colon are significant. All other lines are ignored, meaning that
+explanatory text can be freely mixed with the definitions.
+
+Each file may define more than one mixer; the allocation of mixers to actuators
+is specific to the device reading the mixer definition, and the number of
+actuator outputs generated by a mixer is specific to the mixer.
+
+A mixer begins with a line of the form
+
+	<tag>: <mixer arguments>
+
+The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a 
+multirotor mixer, etc.
+
+Null Mixer
+..........
+
+A null mixer consumes no controls and generates a single actuator output whose
+value is always zero.  Typically a null mixer is used as a placeholder in a
+collection of mixers in order to achieve a specific pattern of actuator outputs.
+
+The null mixer definition has the form:
+
+  Z:
+
+Simple Mixer
+............
+
+A simple mixer combines zero or more control inputs into a single actuator
+output.  Inputs are scaled, and the mixing function sums the result before
+applying an output scaler.
+
+A simple mixer definition begins with:
+
+  M: <control count>
+  O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+
+If <control count> is zero, the sum is effectively zero and the mixer will
+output a fixed value that is <offset> constrained by <lower limit> and <upper
+limit>.
+
+The second line defines the output scaler with scaler parameters as discussed
+above. Whilst the calculations are performed as floating-point operations, the
+values stored in the definition file are scaled by a factor of 10000; i.e. an
+offset of -0.5 is encoded as -5000.
+
+The definition continues with <control count> entries describing the control
+inputs and their scaling, in the form:
+
+  S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+
+The <group> value identifies the control group from which the scaler will read,
+and the <index> value an offset within that group.  These values are specific to
+the device reading the mixer definition.
+
+When used to mix vehicle controls, mixer group zero is the vehicle attitude
+control group, and index values zero through three are normally roll, pitch,
+yaw and thrust respectively.
+
+The remaining fields on the line configure the control scaler with parameters as
+discussed above. Whilst the calculations are performed as floating-point
+operations, the values stored in the definition file are scaled by a factor of
+10000; i.e. an offset of -0.5 is encoded as -5000.
+
+Multirotor Mixer
+................
+
+The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
+into a set of actuator outputs intended to drive motor speed controllers.
+
+The mixer definition is a single line of the form:
+
+R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
+
+The supported geometries include:
+
+  4x - quadrotor in X configuration
+  4+ - quadrotor in + configuration
+  6x - hexcopter in X configuration
+  6+ - hexcopter in + configuration
+  8x - octocopter in X configuration
+  8+ - octocopter in + configuration
+  
+Each of the roll, pitch and yaw scale values determine scaling of the roll,
+pitch and yaw controls relative to the thrust control.  Whilst the calculations
+are performed as floating-point operations, the values stored in the definition
+file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
+
+Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
+thrust input ranges from 0.0 to 1.0.  Output for each actuator is in the 
+range -1.0 to 1.0.
+
+In the case where an actuator saturates, all actuator values are rescaled so that 
+the saturating actuator is limited to 1.0.
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/mixers/easystar.mix b/ROMFS/px4fmu_common/mixers/easystar.mix
new file mode 100644
index 000000000..0051ffdbb
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/easystar.mix
@@ -0,0 +1,31 @@
+EASYSTAR / EASYSTAR II MIXER
+============================
+
+Aileron mixer
+-------------
+One output - would be easy to add support for 2 servos
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+
+Elevator mixer
+------------
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 1  -10000  -10000      0 -10000  10000
+
+Rudder mixer
+------------
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 2  -10000  -10000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/wingwing.mix b/ROMFS/px4fmu_common/mixers/wingwing.mix
new file mode 100644
index 000000000..08333ba5c
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/wingwing.mix
@@ -0,0 +1,51 @@
+Delta-wing mixer for PX4FMU
+===========================
+
+Designed for Wing Wing Z-84
+
+This file defines mixers suitable for controlling a delta wing aircraft using
+PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
+servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
+assumed to be unused.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+See the README for more information on the scaler format.
+
+Elevon mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two elevon servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+The scaling factor for roll inputs is adjusted to implement differential travel
+for the elevons.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  -6000  -6000      0 -10000  10000
+S: 0 1   6500   6500      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  -6000  -6000      0 -10000  10000
+S: 0 1  -6500  -6500      0 -10000  10000
+
+Output 2
+--------
+This mixer is empty.
+
+Z:
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
diff --git a/Tools/px4params/.gitignore b/Tools/.gitignore
index 5d0378b4a..7628bda82 100644
--- a/Tools/px4params/.gitignore
+++ b/Tools/.gitignore
@@ -1,3 +1,2 @@
 parameters.wiki
 parameters.xml
-cookies.txt
\ No newline at end of file
diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh
new file mode 100755
index 000000000..8adc6b6c7
--- /dev/null
+++ b/Tools/check_submodules.sh
@@ -0,0 +1,56 @@
+#!/bin/sh
+
+[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && {
+    # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules
+    echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC"
+    exit 0
+}
+
+if [ -d NuttX/nuttx ];
+	then
+	STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
+	if [ -z "$STATUSRETVAL" ]; then
+		echo "Checked NuttX submodule, correct version found"
+	else
+		echo ""
+		echo ""
+		echo "   NuttX sub repo not at correct version. Try 'git submodule update'"
+		echo "   or follow instructions on http://pixhawk.org/dev/git/submodules"
+		echo ""
+		echo "   DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!"
+		echo ""
+		echo ""
+		echo "New commits required:"
+		echo "$(git submodule summary)"
+		echo ""
+		exit 1
+	fi
+else
+	git submodule init;
+	git submodule update;
+fi
+
+
+if [ -d mavlink/include/mavlink/v1.0 ];
+	then
+	STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<")
+	if [ -z "$STATUSRETVAL" ]; then
+		echo "Checked mavlink submodule, correct version found"
+	else
+		echo ""
+		echo ""
+		echo "mavlink sub repo not at correct version. Try 'git submodule update'"
+		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+		echo ""
+		echo ""
+		echo "New commits required:"
+		echo "$(git submodule summary)"
+		echo ""
+		exit 1
+	fi
+else
+	git submodule init;
+	git submodule update;
+fi
+
+exit 0
diff --git a/Tools/fetch_log.py b/Tools/fetch_log.py
new file mode 100644
index 000000000..edcc6557c
--- /dev/null
+++ b/Tools/fetch_log.py
@@ -0,0 +1,133 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2012, 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.
+#
+############################################################################
+
+#
+# Log fetcher
+#
+# Print list of logs:
+#     python fetch_log.py
+#
+# Fetch log:
+#     python fetch_log.py sess001/log001.bin
+#
+
+import serial, time, sys, os
+
+def wait_for_string(ser, s, timeout=1.0, debug=False):
+    t0 = time.time()
+    buf = []
+    res = []
+    n = 0
+    while (True):
+        c = ser.read()
+        if debug:
+            sys.stderr.write(c)
+        buf.append(c)
+        if len(buf) > len(s):
+            res.append(buf.pop(0))
+            n += 1
+            if n % 10000 == 0:
+                sys.stderr.write(str(n) + "\n")
+        if "".join(buf) == s:
+            break
+        if timeout > 0.0 and time.time() - t0 > timeout:
+            raise Exception("Timeout while waiting for: " + s)
+    return "".join(res)
+
+def exec_cmd(ser, cmd, timeout):
+    ser.write(cmd + "\n")
+    ser.flush()
+    wait_for_string(ser, cmd + "\r\n", timeout)
+    return wait_for_string(ser, "nsh> \x1b[K", timeout)
+
+def ls_dir(ser, dir, timeout=1.0):
+    res = []
+    for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]:
+        res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
+    return res
+
+def list_logs(ser):
+    logs_dir = "/fs/microsd/log"
+    res = []
+    for d in ls_dir(ser, logs_dir):
+        if d[2]:
+            sess_dir = d[0][:-1]
+            for f in ls_dir(ser, logs_dir + "/" + sess_dir):
+                log_file = f[0]
+                log_size = f[1]
+                res.append(sess_dir + "/" + log_file + "\t" + str(log_size))
+    return "\n".join(res)
+
+def fetch_log(ser, fn, timeout):
+    cmd = "dumpfile " + fn
+    ser.write(cmd + "\n")
+    ser.flush()
+    wait_for_string(ser, cmd + "\r\n", timeout, True)
+    res = wait_for_string(ser, "\n", timeout, True)
+    data = []
+    if res.startswith("OK"):
+        size = int(res.split()[1])
+        n = 0
+        print "Reading data:"
+        while (n < size):
+            buf = ser.read(min(size - n, 8192))
+            data.append(buf)
+            n += len(buf)
+            sys.stdout.write(".")
+            sys.stdout.flush()
+        print
+    else:
+        raise Exception("Error reading log")
+    wait_for_string(ser, "nsh> \x1b[K", timeout)
+    return "".join(data)
+
+def main():
+    dev = "/dev/tty.usbmodem1"
+    ser = serial.Serial(dev, "115200", timeout=0.2)
+    if len(sys.argv) < 2:
+        print list_logs(ser)
+    else:
+        log_file = sys.argv[1]
+        data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0)
+        try:
+            os.mkdir(log_file.split("/")[0])
+        except:
+            pass
+        fout = open(log_file, "wb")
+        fout.write(data)
+        fout.close()
+    ser.close()
+
+if __name__ == "__main__":
+    main()
diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh
index 832ee79da..5995d428e 100755
--- a/Tools/fix_code_style.sh
+++ b/Tools/fix_code_style.sh
@@ -16,4 +16,6 @@ astyle \
     --ignore-exclude-errors-x	\
     --lineend=linux		\
     --exclude=EASTL		\
+    --add-brackets		\
+    --max-code-length=120	\
     $*
diff --git a/Tools/fix_code_style_ubuntu.sh b/Tools/fix_code_style_ubuntu.sh
deleted file mode 100755
index 90ab57b89..000000000
--- a/Tools/fix_code_style_ubuntu.sh
+++ /dev/null
@@ -1,19 +0,0 @@
-#!/bin/sh
-astyle \
-    --style=linux		\
-    --indent=force-tab=8	\
-    --indent-cases		\
-    --indent-preprocessor	\
-    --break-blocks=all		\
-    --pad-oper			\
-    --pad-header		\
-    --unpad-paren		\
-    --keep-one-line-blocks	\
-    --keep-one-line-statements	\
-    --align-pointer=name	\
-    --suffix=none		\
-    --lineend=linux		\
-    $*
-    #--ignore-exclude-errors-x	\
-    #--exclude=EASTL		\
-    #--align-reference=name	\
diff --git a/Tools/fsm_visualisation.py b/Tools/fsm_visualisation.py
new file mode 100755
index 000000000..c678ef0f4
--- /dev/null
+++ b/Tools/fsm_visualisation.py
@@ -0,0 +1,201 @@
+#!/usr/bin/env python3
+
+"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table
+
+convert dot code to png using graphviz:
+
+dot fsm.dot -Tpng -o fsm.png
+"""
+
+import argparse
+import re
+
+__author__      = "Julian Oes"
+
+def get_dot_header():
+
+    return """digraph finite_state_machine {
+    graph [ dpi = 300 ];
+    ratio = 1.5
+    node [shape = circle];"""
+
+def get_dot_footer():
+
+    return """}\n"""
+
+def main():
+
+    # parse input arguments
+    parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.')
+    parser.add_argument("-i", "--input-file", default=None, help="choose file to parse")
+    parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file")
+    parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table")
+    args = parser.parse_args()
+    
+    # open source file
+    if args.input_file == None:
+        exit('please specify file')
+    f = open(args.input_file,'r')
+    source = f.read()
+
+    # search for state transition table and extract the table itself
+    #     first look for StateTable::Tran
+    #     then accept anything including newline until {
+    #     but don't accept the definition (without ;)
+    #     then extract anything inside the brackets until };
+    match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source)
+
+    if not match:
+        exit('no state transition table found')
+
+    table_source = match.group(1)
+
+    # bookkeeping for error checking
+    num_errors_found = 0
+
+    states = []
+    events = []
+
+    # first get all states and events
+    for table_line in table_source.split('\n'):
+
+        match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line)
+        if match:
+            states.append(str(match.group(1)))
+            # go to next line
+            continue
+
+        if len(states) == 1:
+            match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line)
+            if match:
+                events.append(str(match.group(1)))
+
+    print('Found %d states and %d events' % (len(states), len(events)))
+
+
+    # keep track of origin state
+    state = None
+
+    # fill dot code in here
+    dot_code = ''
+
+    # create table len(states)xlen(events)
+    transition_table = [[[] for x in range(len(states))] for y in range(len(events))]
+
+    # now fill the transition table and write the dot code
+    for table_line in table_source.split('\n'):
+        
+        # get states
+        #     from: /* NAV_STATE_NONE */
+        #     extract only "NONE"
+        match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line)
+        if match:
+            state = match.group(1)
+            state_index = states.index(state)
+            # go to next line
+            continue
+
+        # can't advance without proper state
+        if state == None:
+            continue
+
+        # get event and next state
+        #     from /* EVENT_READY_REQUESTED */      {ACTION(&Navigator::start_ready), NAV_STATE_READY}
+        #     extract "READY_REQUESTED" and "READY" if there is ACTION
+        match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line)
+
+        # get event and next state
+        #     from      /* EVENT_NONE_REQUESTED */      {NO_ACTION, NAV_STATE_NONE},
+        #     extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION
+        match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line)
+        
+        # ignore lines with brackets only
+        if match_action or match_no_action:
+            
+            # only write arrows for actions
+            if match_action:                
+                event = match_action.group(1)
+                new_state = match_action.group(2)
+                dot_code += '    ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n'
+
+            elif match_no_action:
+                event = match_no_action.group(1)
+                new_state = match_no_action.group(2)
+
+                # check for state changes without action
+                if state != new_state:
+                    print('Error: no action but state change:')
+                    print('State: ' + state + ' changed to: ' + new_state)
+                    print(table_line)
+                    num_errors_found += 1
+
+            # check for wrong events
+            if event not in events:
+                print('Error: unknown event: ' + event)
+                print(table_line)
+                num_errors_found += 1
+
+            # check for wrong new states
+            if new_state not in states:
+                print('Error: unknown new state: ' + new_state)
+                print(table_line)
+                num_errors_found += 1
+
+            # save new state in transition table
+            event_index = events.index(event)
+
+            # bold for action
+            if match_action:
+                transition_table[event_index][state_index] = '**' + new_state + '**'
+            else:
+                transition_table[event_index][state_index] = new_state
+
+    
+    
+    # assemble dot code
+    dot_code = get_dot_header() + dot_code + get_dot_footer()
+    
+    # write or print dot file
+    if args.dot_file:
+        f = open(args.dot_file,'w')
+        f.write(dot_code)
+        print('Wrote dot file')
+    else:
+        print('##########Dot-start##########')
+        print(dot_code)
+        print('##########Dot-end############')
+    
+
+    # assemble doku wiki table
+    table_code = '| ^ '
+    # start with header of all states
+    for state in states:
+        table_code += state + ' ^ '
+
+    table_code += '\n'
+
+    # add events and new states
+    for event, row in zip(events, transition_table):
+        table_code += '^ ' + event + ' | ' 
+        for new_state in row:
+            table_code += new_state + ' | '
+        table_code += '\n'
+
+    # write or print wiki table
+    if args.table_file:
+        f = open(args.table_file,'w')
+        f.write(table_code)
+        print('Wrote table file')
+    else:
+        print('##########Table-start########')
+        print(table_code)
+        print('##########Table-end##########')
+
+    # report obvous errors
+    if num_errors_found:
+        print('Obvious errors found: %d' % num_errors_found)
+    else:
+        print('No obvious errors found')
+
+if __name__ == '__main__':
+    main()
diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md
index a23b44799..50dcd2e29 100644
--- a/Tools/px4params/README.md
+++ b/Tools/px4params/README.md
@@ -1,9 +1 @@
-h1. PX4 Parameters Processor
-
-It's designed to scan PX4 source codes, find declarations of tunable parameters,
-and generate the list in various formats.
-
-Currently supported formats are:
-
-* XML for the parametric UI generator
-* Human-readable description in DokuWiki format
+This folder contains a python library used by px_process_params.py
diff --git a/Tools/px4params/__init__.py b/Tools/px4params/__init__.py
new file mode 100644
index 000000000..3a9f1e2c6
--- /dev/null
+++ b/Tools/px4params/__init__.py
@@ -0,0 +1 @@
+__all__ = ["srcscanner", "srcparser", "xmlout", "dokuwikiout", "dokuwikirpc"]
\ No newline at end of file
diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py
index c5cf65ea6..77e0ef53d 100644
--- a/Tools/px4params/dokuwikiout.py
+++ b/Tools/px4params/dokuwikiout.py
@@ -1,62 +1,44 @@
-import output
 from xml.sax.saxutils import escape
+import codecs
 
-class DokuWikiOutput(output.Output):
-    def Generate(self, groups):
-        pre_text = """<?xml version='1.0'?>
-            <methodCall>
-              <methodName>wiki.putPage</methodName>
-                  <params>
-              <param> 
-                <value>
-                  <string>:firmware:parameters</string>
-                </value>
-              </param> 
-                  <param> 
-                <value>
-                  <string>"""
-        result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values."
+class DokuWikiTablesOutput():
+    def __init__(self, groups):
+        result = ("====== Parameter Reference ======\n"
+                  "<note>**This list is auto-generated from the source code** and contains the most recent parameter documentation.</note>\n"
+                  "\n")
         for group in groups:
             result += "==== %s ====\n\n" % group.GetName()
-            result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n"
-            result += "^ Name            ^ Description    ^ Min    ^ Max    ^ Default   ^ Comment    ^\n"
+            result += "|< 100% 25% 45% 10% 10% 10% >|\n"
+            result += "^ Name ^ Description ^  Min ^  Max ^  Default ^\n"
+            result += "^ :::  ^ Comment ^^^^\n"
             for param in group.GetParams():
                 code = param.GetFieldValue("code")
                 name = param.GetFieldValue("short_desc")
-                name = name.replace("\n", "")
-                result += "| %s   | %s  " % (code, name)
                 min_val = param.GetFieldValue("min")
-                if min_val is not None:
-                    result += " | %s  " % min_val
-                else:
-                    result += " | "
                 max_val = param.GetFieldValue("max")
-                if max_val is not None:
-                    result += " | %s  " % max_val
-                else:
-                    result += " | "
                 def_val = param.GetFieldValue("default")
-                if def_val is not None:
-                    result += "| %s  " % def_val
-                else:
-                    result += " | "
                 long_desc = param.GetFieldValue("long_desc")
-                if long_desc is not None:
-                    long_desc = long_desc.replace("\n", "")
-                    result += "| %s  " % long_desc
+
+                if name == code:
+                    name = ""
                 else:
-                    result += " | "
-                result += " |\n"
+                    name = name.replace("\n", " ")
+                    name = name.replace("|", "%%|%%")
+                    name = name.replace("^", "%%^%%")
+
+                result += "| **%s** |" % code
+                result += " %s |" % name
+                result += "  %s |" % (min_val or "")
+                result += "  %s |" % (max_val or "")
+                result += "  %s |" % (def_val or "")
+                result += "\n"
+
+                if long_desc is not None:
+                    result += "| ::: | <div>%s</div> ||||\n" % long_desc
+
             result += "\n"
-        post_text = """</string>
-        </value>
-      </param> 
-           <param> 
-        <value>
-            <name>sum</name>
-            <string>Updated parameters automagically from code.</string>
-        </value>
-      </param> 
-    </params> 
-    </methodCall>"""
-        return pre_text + escape(result) + post_text
+        self.output = result;
+
+    def Save(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            f.write(self.output)
diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/dokuwikiout_listings.py
deleted file mode 100644
index 33f76b415..000000000
--- a/Tools/px4params/dokuwikiout_listings.py
+++ /dev/null
@@ -1,27 +0,0 @@
-import output
-
-class DokuWikiOutput(output.Output):
-    def Generate(self, groups):
-        result = ""
-        for group in groups:
-            result += "==== %s ====\n\n" % group.GetName()
-            for param in group.GetParams():
-                code = param.GetFieldValue("code")
-                name = param.GetFieldValue("short_desc")
-                if code != name:
-                    name = "%s (%s)" % (name, code)
-                result += "=== %s ===\n\n" % name
-                long_desc = param.GetFieldValue("long_desc")
-                if long_desc is not None:
-                    result += "%s\n\n" % long_desc
-                min_val = param.GetFieldValue("min")
-                if min_val is not None:
-                    result += "* Minimal value: %s\n" % min_val
-                max_val = param.GetFieldValue("max")
-                if max_val is not None:
-                    result += "* Maximal value: %s\n" % max_val
-                def_val = param.GetFieldValue("default")
-                if def_val is not None:
-                    result += "* Default value: %s\n" % def_val
-                result += "\n"
-        return result
diff --git a/Tools/px4params/dokuwikirpc.py b/Tools/px4params/dokuwikirpc.py
new file mode 100644
index 000000000..407d306fd
--- /dev/null
+++ b/Tools/px4params/dokuwikirpc.py
@@ -0,0 +1,16 @@
+try:
+    import xmlrpclib
+except ImportError:
+    import xmlrpc.client as xmlrpclib 
+
+# See https://www.dokuwiki.org/devel:xmlrpc for a list of available functions!
+# Usage example:
+#     xmlrpc = dokuwikirpc.get_xmlrpc(url, username, password)
+#     print(xmlrpc.dokuwiki.getVersion())
+
+def get_xmlrpc(url, username, password):
+    #proto, url = url.split("://")
+    #url = proto + "://" + username + ":" + password + "@" + url + "/lib/exe/xmlrpc.php"
+    url += "/lib/exe/xmlrpc.php?u=" + username + "&p=" + password
+    
+    return xmlrpclib.ServerProxy(url)
diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py
deleted file mode 100644
index c09246871..000000000
--- a/Tools/px4params/output.py
+++ /dev/null
@@ -1,5 +0,0 @@
-class Output(object):
-    def Save(self, groups, fn):
-        data = self.Generate(groups)
-        with open(fn, 'w') as f:
-            f.write(data)
diff --git a/Tools/px4params/parser.py b/Tools/px4params/srcparser.py
index 251be672f..0a4d21d26 100644
--- a/Tools/px4params/parser.py
+++ b/Tools/px4params/srcparser.py
@@ -28,8 +28,7 @@ class ParameterGroup(object):
         state of the parser.
         """
         return sorted(self.params,
-                cmp=lambda x, y: cmp(x.GetFieldValue("code"),
-                    y.GetFieldValue("code")))
+                key=lambda x: x.GetFieldValue("code"))
 
 class Parameter(object):
     """
@@ -45,6 +44,7 @@ class Parameter(object):
         "default": 6,
         "min": 5,
         "max": 4,
+        "unit": 3,
         # all others == 0 (sorted alphabetically)
     }
 
@@ -61,9 +61,10 @@ class Parameter(object):
         """
         Return list of existing field codes in convenient order
         """
-        return sorted(self.fields.keys(),
-                cmp=lambda x, y: cmp(self.priority.get(y, 0),
-                    self.priority.get(x, 0)) or cmp(x, y))
+        keys = self.fields.keys()
+        keys = sorted(keys)
+        keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
+        return keys
 
     def GetFieldValue(self, code):
         """
@@ -71,7 +72,7 @@ class Parameter(object):
         """
         return self.fields.get(code)
 
-class Parser(object):
+class SourceParser(object):
     """
     Parses provided data and stores all found parameters internally.
     """
@@ -86,7 +87,7 @@ class Parser(object):
     re_is_a_number = re.compile(r'^-?[0-9\.]')
     re_remove_dots = re.compile(r'\.+$')
 
-    valid_tags = set(["min", "max", "group"])
+    valid_tags = set(["group", "min", "max", "unit"])
 
     # Order of parameter groups
     priority = {
@@ -197,7 +198,7 @@ class Parser(object):
                             if tag == "group":
                                 group = tags[tag]
                             elif tag not in self.valid_tags:
-                                sys.stderr.write("Skipping invalid"
+                                sys.stderr.write("Skipping invalid "
                                         "documentation tag: '%s'\n" % tag)
                             else:
                                 param.SetField(tag, tags[tag])
@@ -214,7 +215,7 @@ class Parser(object):
         object. Note that returned object is not a copy. Modifications affect
         state of the parser.
         """
-        return sorted(self.param_groups.values(),
-                cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0),
-                    self.priority.get(x.GetName(), 0)) or cmp(x.GetName(),
-                        y.GetName()))
+        groups = self.param_groups.values()
+        groups = sorted(groups, key=lambda x: x.GetName())
+        groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)
+        return groups
diff --git a/Tools/px4params/scanner.py b/Tools/px4params/srcscanner.py
index b5a1af47c..d7eca72d7 100644
--- a/Tools/px4params/scanner.py
+++ b/Tools/px4params/srcscanner.py
@@ -1,26 +1,22 @@
 import os
 import re
+import codecs
 
-class Scanner(object):
+class SourceScanner(object):
     """
     Traverses directory tree, reads all source files, and passes their contents
     to the Parser.
     """
 
-    re_file_extension = re.compile(r'\.([^\.]+)$')
-
     def ScanDir(self, srcdir, parser):
         """
         Scans provided path and passes all found contents to the parser using
         parser.Parse method.
         """
-        extensions = set(parser.GetSupportedExtensions())
+        extensions = tuple(parser.GetSupportedExtensions())
         for dirname, dirnames, filenames in os.walk(srcdir):
             for filename in filenames:
-                m = self.re_file_extension.search(filename)
-                if m:
-                    ext = m.group(1)
-                    if ext in extensions:
+                if filename.endswith(extensions):
                         path = os.path.join(dirname, filename)
                         self.ScanFile(path, parser)
 
@@ -29,6 +25,6 @@ class Scanner(object):
         Scans provided file and passes its contents to the parser using
         parser.Parse method.
         """
-        with open(path, 'r') as f:
+        with codecs.open(path, 'r', 'utf-8') as f:
             contents = f.read()
         parser.Parse(contents)
diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py
index d56802b15..e845cd1b1 100644
--- a/Tools/px4params/xmlout.py
+++ b/Tools/px4params/xmlout.py
@@ -1,8 +1,8 @@
-import output
 from xml.dom.minidom import getDOMImplementation
+import codecs
 
-class XMLOutput(output.Output):
-    def Generate(self, groups):
+class XMLOutput():
+    def __init__(self, groups):
         impl = getDOMImplementation()
         xml_document = impl.createDocument(None, "parameters", None)
         xml_parameters = xml_document.documentElement
@@ -19,4 +19,8 @@ class XMLOutput(output.Output):
                     xml_param.appendChild(xml_field)
                     xml_value = xml_document.createTextNode(value)
                     xml_field.appendChild(xml_value)
-        return xml_document.toprettyxml(indent="    ", newl="\n", encoding="utf-8")
+        self.xml_document = xml_document
+
+    def Save(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            self.xml_document.writexml(f, indent="    ", addindent="    ", newl="\n")
diff --git a/Tools/px4params/xmlrpc.sh b/Tools/px4params/xmlrpc.sh
deleted file mode 100644
index 36c52ff71..000000000
--- a/Tools/px4params/xmlrpc.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-python px_process_params.py
-
-rm cookies.txt
-curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login
-curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php"
diff --git a/Tools/px_generate_xml.sh b/Tools/px_generate_xml.sh
new file mode 100644
index 000000000..65f0c95da
--- /dev/null
+++ b/Tools/px_generate_xml.sh
@@ -0,0 +1,2 @@
+#!/bin/sh
+python px_process_params.py --xml
diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py
new file mode 100644
index 000000000..12128a997
--- /dev/null
+++ b/Tools/px_process_params.py
@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# PX4 paramers processor (main executable file)
+#
+# This tool scans the PX4 source code for declarations of tunable parameters
+# and outputs the list in various formats.
+#
+# Currently supported formats are:
+#   * XML for the parametric UI generator
+#   * Human-readable description in DokuWiki page format
+#
+# This tool also allows to automatically upload the human-readable version
+# to the DokuWiki installation via XML-RPC.
+#
+
+from __future__ import print_function
+import sys
+import os
+import argparse
+from px4params import srcscanner, srcparser, xmlout, dokuwikiout, dokuwikirpc
+
+def main():
+    # Parse command line arguments
+    parser = argparse.ArgumentParser(description="Process parameter documentation.")
+    parser.add_argument("-s", "--src-path",
+                        default="../src",
+                        metavar="PATH",
+                        help="path to source files to scan for parameters")
+    parser.add_argument("-x", "--xml",
+                        nargs='?',
+                        const="parameters.xml",
+                        metavar="FILENAME",
+                        help="Create XML file"
+                             " (default FILENAME: parameters.xml)")
+    parser.add_argument("-w", "--wiki",
+                        nargs='?',
+                        const="parameters.wiki",
+                        metavar="FILENAME",
+                        help="Create DokuWiki file"
+                             " (default FILENAME: parameters.wiki)")
+    parser.add_argument("-u", "--wiki-update",
+                        nargs='?',
+                        const="firmware:parameters",
+                        metavar="PAGENAME",
+                        help="Update DokuWiki page"
+                             " (default PAGENAME: firmware:parameters)")
+    parser.add_argument("--wiki-url",
+                        default="https://pixhawk.org",
+                        metavar="URL",
+                        help="DokuWiki URL"
+                             " (default: https://pixhawk.org)")
+    parser.add_argument("--wiki-user",
+                        default=os.environ.get('XMLRPCUSER', None),
+                        metavar="USERNAME",
+                        help="DokuWiki XML-RPC user name"
+                             " (default: $XMLRPCUSER environment variable)")
+    parser.add_argument("--wiki-pass",
+                        default=os.environ.get('XMLRPCPASS', None),
+                        metavar="PASSWORD",
+                        help="DokuWiki XML-RPC user password"
+                             " (default: $XMLRPCUSER environment variable)")
+    parser.add_argument("--wiki-summary",
+                        metavar="SUMMARY",
+                        default="Automagically updated parameter documentation from code.",
+                        help="DokuWiki page edit summary")
+    args = parser.parse_args()
+
+    # Check for valid command
+    if not (args.xml or args.wiki or args.wiki_update):
+        print("Error: You need to specify at least one output method!\n")
+        parser.print_usage()
+        sys.exit(1)
+
+    # Initialize source scanner and parser
+    scanner = srcscanner.SourceScanner()
+    parser = srcparser.SourceParser()
+
+    # Scan directories, and parse the files
+    print("Scanning source path " + args.src_path)
+    scanner.ScanDir(args.src_path, parser)
+    param_groups = parser.GetParamGroups()
+
+    # Output to XML file
+    if args.xml:
+        print("Creating XML file " + args.xml)
+        out = xmlout.XMLOutput(param_groups)
+        out.Save(args.xml)
+
+    # Output to DokuWiki tables
+    if args.wiki or args.wiki_update:
+        out = dokuwikiout.DokuWikiTablesOutput(param_groups)
+        if args.wiki:
+            print("Creating wiki file " + args.wiki)
+            out.Save(args.wiki)
+        if args.wiki_update:
+            if args.wiki_user and args.wiki_pass:
+                print("Updating wiki page " + args.wiki_update)
+                xmlrpc = dokuwikirpc.get_xmlrpc(args.wiki_url, args.wiki_user, args.wiki_pass)
+                xmlrpc.wiki.putPage(args.wiki_update, out.output, {'sum': args.wiki_summary})
+            else:
+                print("Error: You need to specify DokuWiki XML-RPC username and password!")
+
+    print("All done!")
+
+
+if __name__ == "__main__":
+    main()
diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py
new file mode 100644
index 000000000..fcc40b09e
--- /dev/null
+++ b/Tools/px_romfs_pruner.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+#   Author: 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
+# 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.
+#
+############################################################################
+
+
+"""
+px_romfs_pruner.py:
+Delete all comments and newlines before ROMFS is converted to an image
+"""
+
+from __future__ import print_function
+import argparse
+import os
+
+
+def main():
+
+        # Parse commandline arguments
+        parser = argparse.ArgumentParser(description="ROMFS pruner.")
+        parser.add_argument('--folder', action="store", help="ROMFS scratch folder.")
+        args = parser.parse_args()
+
+        print("Pruning ROMFS files.")
+
+        # go through
+        for (root, dirs, files) in os.walk(args.folder):
+                for file in files:
+                        # only prune text files
+                        if ".zip" in file or ".bin" in file or ".swp" in file:
+                                continue
+
+                        file_path = os.path.join(root, file)
+
+                        # read file line by line
+                        pruned_content = ""
+                        with open(file_path, "r") as f:
+                                for line in f:
+
+                                        # handle mixer files differently than startup files
+                                        if file_path.endswith(".mix"):
+                                                if line.startswith(("Z:", "M:", "R: ", "O:", "S:")):
+                                                        pruned_content += line
+                                        else:
+                                                if not line.isspace() and not line.strip().startswith("#"):
+                                                        pruned_content += line
+
+                        # overwrite old scratch file
+                        with open(file_path, "w") as f:
+                                f.write(pruned_content)
+
+
+if __name__ == '__main__':
+        main()
diff --git a/Tools/px_update_wiki.sh b/Tools/px_update_wiki.sh
new file mode 100644
index 000000000..d66bb9e10
--- /dev/null
+++ b/Tools/px_update_wiki.sh
@@ -0,0 +1,2 @@
+# Remember to set the XMLRPCUSER and XMLRPCPASS environment variables
+python px_process_params.py --wiki-update
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index e4a8b3c05..cd7884f6d 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -63,6 +63,7 @@ import zlib
 import base64
 import time
 import array
+import os
 
 from sys import platform as _platform
 
@@ -389,18 +390,22 @@ class uploader(object):
                     self.otp_pid = self.otp[12:8:-1]
                     self.otp_coa = self.otp[32:160]
                     # show user:
-                    print("type: " + self.otp_id.decode('Latin-1'))
-                    print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
-                    print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
-                    print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
-                    print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
-                    print("sn: ", end='')
-                    for byte in range(0,12,4):
-                        x = self.__getSN(byte)
-                        x = x[::-1]  # reverse the bytes
-                        self.sn  = self.sn + x
-                        print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
-                    print('')
+                    try:
+                            print("type: " + self.otp_id.decode('Latin-1'))
+                            print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
+                            print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
+                            print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
+                            print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
+                            print("sn: ", end='')
+                            for byte in range(0,12,4):
+                                    x = self.__getSN(byte)
+                                    x = x[::-1]  # reverse the bytes
+                                    self.sn  = self.sn + x
+                                    print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
+                            print('')
+                    except Exception:
+                            # ignore bad character encodings
+                            pass
                 print("erase...")
                 self.__erase()
 
@@ -445,6 +450,12 @@ parser.add_argument('--baud', action="store", type=int, default=115200, help="Ba
 parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
 args = parser.parse_args()
 
+# warn people about ModemManager which interferes badly with Pixhawk
+if os.path.exists("/usr/sbin/ModemManager"):
+        print("==========================================================================================================")
+        print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)")
+        print("==========================================================================================================")
+
 # Load the firmware file
 fw = firmware(args.firmware)
 print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore
index 1618faf58..87b314c61 100644
--- a/Tools/tests-host/.gitignore
+++ b/Tools/tests-host/.gitignore
@@ -1,3 +1,4 @@
 ./obj/*
 mixer_test
-sbus2_test
\ No newline at end of file
+sbus2_test
+autodeclination_test
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile
index 15ccf1594..6e1d08fc1 100644
--- a/Tools/tests-host/Makefile
+++ b/Tools/tests-host/Makefile
@@ -1,9 +1,9 @@
 
 CC=g++
 CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
-	-I../../src -D__EXPORT="" -Dnullptr="0" -lm
+	-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
 
-all: mixer_test sbus2_test
+all: mixer_test sbus2_test autodeclination_test
 
 MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
 		../../src/systemcmds/tests/test_conv.cpp \
@@ -20,13 +20,20 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
 		hrt.cpp \
 		sbus2_test.cpp
 
+AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \
+		hrt.cpp \
+		autodeclination_test.cpp
+
 mixer_test: $(MIXER_FILES)
 	$(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
 
 sbus2_test: $(SBUS2_FILES)
 	$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
 
+autodeclination_test: $(SBUS2_FILES)
+	$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
+
 .PHONY: clean
 
 clean:
-	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test
\ No newline at end of file
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp
new file mode 100644
index 000000000..93bc340bb
--- /dev/null
+++ b/Tools/tests-host/autodeclination_test.cpp
@@ -0,0 +1,28 @@
+
+#include <stdio.h>
+#include <stdlib.h> 
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <px4iofirmware/px4io.h>
+#include "../../src/systemcmds/tests/tests.h"
+#include <geo/geo.h>
+
+int main(int argc, char *argv[]) {
+	warnx("autodeclination test started");
+
+	if (argc < 3)
+		errx(1, "Need lat/lon!");
+
+	char* p_end;
+
+	float lat = strtod(argv[1], &p_end);
+	float lon = strtod(argv[2], &p_end);
+
+	float declination = get_mag_declination(lat, lon);
+
+	printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination);
+
+}
diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp
index e311617f9..06499afd0 100644
--- a/Tools/tests-host/mixer_test.cpp
+++ b/Tools/tests-host/mixer_test.cpp
@@ -11,4 +11,4 @@ int main(int argc, char *argv[]) {
 	test_mixer(3, args);
 
 	test_conv(1, args);
-}
\ No newline at end of file
+}
diff --git a/makefiles/board_aerocore.mk b/makefiles/board_aerocore.mk
new file mode 100644
index 000000000..6f4b93266
--- /dev/null
+++ b/makefiles/board_aerocore.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the Gumstix AeroCore
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH			 = CORTEXM4F
+CONFIG_BOARD			 = AEROCORE
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk
new file mode 100644
index 000000000..53a2ad1ab
--- /dev/null
+++ b/makefiles/config_aerocore_default.mk
@@ -0,0 +1,125 @@
+#
+# Makefile for the AeroCore *default* configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/px4fmu
+MODULES		+= drivers/boards/aerocore
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/gps
+MODULES		+= drivers/hil
+MODULES		+= modules/sensors
+
+#
+# System commands
+#
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/dumpfile
+
+#
+# General system control
+#
+MODULES		+= modules/commander
+MODULES		+= modules/navigator
+MODULES		+= modules/mavlink
+
+#
+# Estimation modules (EKF/ SO3 / other filters)
+#
+MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/attitude_estimator_so3
+MODULES		+= modules/ekf_att_pos_estimator
+MODULES		+= modules/position_estimator_inav
+
+#
+# Vehicle Control
+#
+MODULES		+= modules/fw_pos_control_l1
+MODULES		+= modules/fw_att_control
+MODULES		+= modules/mc_att_control
+MODULES		+= modules/mc_pos_control
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+MODULES		+= modules/dataman
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
+MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
+
+#
+# Demo apps
+#
+#MODULES		+= examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+MODULES		+= examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES		+= examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES		+= examples/px4_mavlink_debug
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+BUILTIN_COMMANDS := \
+    $(call _B, hello, , 2048, hello_main) \
+    $(call _B, i2c, , 2048, i2c_main)
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 43a03f4f7..cc0958c29 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -21,11 +21,11 @@ MODULES		+= drivers/px4fmu
 MODULES		+= drivers/boards/px4fmu-v1
 MODULES		+= drivers/ardrone_interface
 MODULES		+= drivers/l3gd20
-#MODULES		+= drivers/bma180
 MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
+MODULES		+= drivers/ll40ls
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
@@ -44,7 +44,6 @@ MODULES		+= modules/sensors
 #
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/bl_update
-MODULES		+= systemcmds/boardinfo
 MODULES		+= systemcmds/i2c
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
@@ -54,10 +53,10 @@ MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/top
-MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
-MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/dumpfile
+MODULES		+= systemcmds/ver
 
 #
 # General system control
@@ -65,27 +64,22 @@ MODULES		+= systemcmds/hw_ver
 MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
-MODULES		+= modules/mavlink_onboard
 MODULES		+= modules/gpio_led
 
 #
-# Estimation modules (EKF/ SO3 / other filters)
+# Estimation modules (EKF / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
-MODULES		+= modules/attitude_estimator_so3
-MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
-#MODULES		+= examples/flow_position_estimator
 
 #
 # Vehicle Control
 #
 MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
-MODULES		+= modules/multirotor_att_control
-MODULES		+= modules/multirotor_pos_control
-#MODULES		+= examples/flow_position_control
-#MODULES		+= examples/flow_speed_control
+MODULES		+= modules/mc_att_control
+MODULES		+= modules/mc_pos_control
 
 #
 # Logging
@@ -105,6 +99,7 @@ MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/controllib
 MODULES		+= modules/uORB
+MODULES		+= modules/dataman
 
 #
 # Libraries
@@ -113,9 +108,10 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
 
 #
 # Demo apps
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index dc9208339..adfbc2b7d 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -27,18 +27,20 @@ MODULES		+= drivers/l3gd20
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
+MODULES		+= drivers/sf0x
+MODULES		+= drivers/ll40ls
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
-MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
 MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 MODULES		+= drivers/mkblctrl
+MODULES		+= drivers/pca8574
 
 
 # Needs to be burned to the ground and re-written; for now,
@@ -62,7 +64,8 @@ MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
-MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/dumpfile
+MODULES		+= systemcmds/ver
 
 #
 # General system control
@@ -70,14 +73,14 @@ MODULES		+= systemcmds/hw_ver
 MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
-MODULES		+= modules/mavlink_onboard
+MODULES		+= modules/gpio_led
 
 #
 # Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/attitude_estimator_so3
-MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 MODULES		+= examples/flow_position_estimator
 
@@ -87,8 +90,8 @@ MODULES		+= examples/flow_position_estimator
 #MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
 MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
-MODULES		+= modules/multirotor_att_control
-MODULES		+= modules/multirotor_pos_control
+MODULES		+= modules/mc_att_control
+MODULES		+= modules/mc_pos_control
 
 #
 # Logging
@@ -108,6 +111,7 @@ MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/controllib
 MODULES		+= modules/uORB
+MODULES		+= modules/dataman
 
 #
 # Libraries
@@ -116,9 +120,10 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
 
 #
 # Demo apps
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 4f997e96b..395a8f2ac 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -24,12 +24,14 @@ MODULES		+= drivers/lsm303d
 MODULES		+= drivers/l3gd20
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
+MODULES		+= drivers/pca8574
+MODULES		+= drivers/roboclaw
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
-MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/ver
 
 #
 # Library modules
@@ -40,6 +42,12 @@ MODULES		+= modules/uORB
 LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/conversion
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
 
 #
 # Transitional support - add commands from the NuttX export archive.
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index cb20d9cd1..60602e76f 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -113,7 +113,7 @@ endif
 $(info %  GIT_DESC            = $(GIT_DESC))
 
 #
-# Set a default target so that included makefiles or errors here don't 
+# Set a default target so that included makefiles or errors here don't
 # cause confusion.
 #
 # XXX We could do something cute here with $(DEFAULT_GOAL) if it's not one
@@ -177,7 +177,7 @@ GLOBAL_DEPS		+= $(MAKEFILE_LIST)
 #
 # Extra things we should clean
 #
-EXTRA_CLEANS		 = 
+EXTRA_CLEANS		 =
 
 
 #
@@ -355,6 +355,9 @@ ROMFS_OBJ		 = $(ROMFS_CSRC:.c=.o)
 LIBS			+= $(ROMFS_OBJ)
 LINK_DEPS		+= $(ROMFS_OBJ)
 
+# Remove all comments from startup and mixer files
+ROMFS_PRUNER	 = $(PX4_BASE)/Tools/px_romfs_pruner.py
+
 # Turn the ROMFS image into an object file
 $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
 	$(call BIN_TO_OBJ,$<,$@,romfs_img)
@@ -368,10 +371,13 @@ $(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS)
 $(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS)
 	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)
 	$(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH)
+# delete all files in ROMFS_SCRATCH which start with a . or end with a ~
+	$(Q) $(RM) $(ROMFS_SCRATCH)/*/.[!.]* $(ROMFS_SCRATCH)/*/*~
 ifneq ($(ROMFS_EXTRA_FILES),)
 	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
 	$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
 endif
+	$(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH)
 
 EXTRA_CLEANS		+= $(ROMGS_OBJ) $(ROMFS_IMG)
 
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 183b143d6..6a092ef6b 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -46,6 +46,7 @@ export PX4_MODULE_SRC	 = $(abspath $(PX4_BASE)/src)/
 export PX4_LIB_DIR	 = $(abspath $(PX4_BASE)/src/lib)/
 export PX4_MK_DIR	 = $(abspath $(PX4_BASE)/makefiles)/
 export NUTTX_SRC	 = $(abspath $(PX4_BASE)/NuttX/nuttx)/
+export MAVLINK_SRC	 = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
 export NUTTX_APP_SRC	 = $(abspath $(PX4_BASE)/NuttX/apps)/
 export MAVLINK_SRC	 = $(abspath $(PX4_BASE)/mavlink)/
 export ROMFS_SRC	 = $(abspath $(PX4_BASE)/ROMFS)/
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index bb729e103..d8d45d34e 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -1,5 +1,5 @@
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
@@ -48,6 +48,16 @@ NM			 = $(CROSSDEV)nm
 OBJCOPY			 = $(CROSSDEV)objcopy
 OBJDUMP			 = $(CROSSDEV)objdump
 
+# Check if the right version of the toolchain is available
+#
+CROSSDEV_VER_SUPPORTED	 = 4.7
+CROSSDEV_VER_FOUND	 = $(shell $(CC) -dumpversion)
+
+ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)))
+$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x)
+endif
+
+
 # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
 
 MAXOPTIMIZATION		 ?= -O3
@@ -76,7 +86,7 @@ ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
 ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
 			   -ffixed-r10
 
-ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = 
+ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
 
 # Pick the right set of flags for the architecture.
 #
@@ -125,7 +135,15 @@ ARCHWARNINGS		 = -Wall \
 			   -Wlogical-op \
 			   -Wmissing-declarations \
 			   -Wpacked \
-			   -Wno-unused-parameter
+			   -Wno-unused-parameter \
+			   -Werror=format-security \
+			   -Werror=array-bounds \
+			   -Wfatal-errors \
+			   -Wformat=1 \
+			   -Werror=unused-but-set-variable \
+			   -Werror=unused-variable \
+			   -Werror=double-promotion \
+			   -Werror=reorder
 #   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
 #   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
 #   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
@@ -142,7 +160,8 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 
 # C++-specific warnings
 #
-ARCHWARNINGSXX		 = $(ARCHWARNINGS)
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-missing-field-initializers
 
 # pull in *just* libm from the toolchain ... this is grody
 LIBM			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)
@@ -260,7 +279,7 @@ define SYM_TO_BIN
 	$(Q) $(OBJCOPY) -O binary $1 $2
 endef
 
-# Take the raw binary $1 and make it into an object file $2. 
+# Take the raw binary $1 and make it into an object file $2.
 # The symbol $3 points to the beginning of the file, and $3_len
 # gives its length.
 #
diff --git a/mavlink/include/mavlink/config.h b/mavlink/include/mavlink/config.h
deleted file mode 100644
index db7db0d7d..000000000
--- a/mavlink/include/mavlink/config.h
+++ /dev/null
@@ -1 +0,0 @@
-#define MAVLINK_VERSION "1.0.7"
diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
new file mode 160000
+Subproject d1ebe85eb6bb06d0078f3e0b8144adb13126362
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
deleted file mode 100644
index 2acb7965c..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
+++ /dev/null
@@ -1,205 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol generated from ardupilotmega.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef ARDUPILOTMEGA_H
-#define ARDUPILOTMEGA_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// 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}
-#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}
-#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}}}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_ARDUPILOTMEGA
-
-// 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
-enum LIMITS_STATE
-{
-	LIMITS_INIT=0, /*  pre-initialization | */
-	LIMITS_DISABLED=1, /*  disabled | */
-	LIMITS_ENABLED=2, /*  checking limits | */
-	LIMITS_TRIGGERED=3, /*  a limit has been breached | */
-	LIMITS_RECOVERING=4, /*  taking action eg. RTL | */
-	LIMITS_RECOVERED=5, /*  we're no longer in breach of a limit | */
-	LIMITS_STATE_ENUM_END=6, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_LIMIT_MODULE
-#define HAVE_ENUM_LIMIT_MODULE
-enum LIMIT_MODULE
-{
-	LIMIT_GPSLOCK=1, /*  pre-initialization | */
-	LIMIT_GEOFENCE=2, /*  disabled | */
-	LIMIT_ALTITUDE=4, /*  checking limits | */
-	LIMIT_MODULE_ENUM_END=5, /*  | */
-};
-#endif
-
-/** @brief Flags in RALLY_POINT message */
-#ifndef HAVE_ENUM_RALLY_FLAGS
-#define HAVE_ENUM_RALLY_FLAGS
-enum RALLY_FLAGS
-{
-	FAVORABLE_WIND=1, /* Flag set when requiring favorable winds for landing.  | */
-	LAND_IMMEDIATELY=2, /* Flag set when plane is to immediately descend to break altitude and land without GCS intervention.  Flag not set when plane is to loiter at Rally point until commanded to land. | */
-	RALLY_FLAGS_ENUM_END=3, /*  | */
-};
-#endif
-
-#include "../common/common.h"
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_sensor_offsets.h"
-#include "./mavlink_msg_set_mag_offsets.h"
-#include "./mavlink_msg_meminfo.h"
-#include "./mavlink_msg_ap_adc.h"
-#include "./mavlink_msg_digicam_configure.h"
-#include "./mavlink_msg_digicam_control.h"
-#include "./mavlink_msg_mount_configure.h"
-#include "./mavlink_msg_mount_control.h"
-#include "./mavlink_msg_mount_status.h"
-#include "./mavlink_msg_fence_point.h"
-#include "./mavlink_msg_fence_fetch_point.h"
-#include "./mavlink_msg_fence_status.h"
-#include "./mavlink_msg_ahrs.h"
-#include "./mavlink_msg_simstate.h"
-#include "./mavlink_msg_hwstatus.h"
-#include "./mavlink_msg_radio.h"
-#include "./mavlink_msg_limits_status.h"
-#include "./mavlink_msg_wind.h"
-#include "./mavlink_msg_data16.h"
-#include "./mavlink_msg_data32.h"
-#include "./mavlink_msg_data64.h"
-#include "./mavlink_msg_data96.h"
-#include "./mavlink_msg_rangefinder.h"
-#include "./mavlink_msg_airspeed_autocal.h"
-#include "./mavlink_msg_rally_point.h"
-#include "./mavlink_msg_rally_fetch_point.h"
-#include "./mavlink_msg_ahrs2.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // ARDUPILOTMEGA_H
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h
deleted file mode 100644
index 551938f04..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from ardupilotmega.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "ardupilotmega.h"
-
-#endif // MAVLINK_H
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h
deleted file mode 100644
index c3ead1140..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE AHRS PACKING
-
-#define MAVLINK_MSG_ID_AHRS 163
-
-typedef struct __mavlink_ahrs_t
-{
- float omegaIx; ///< X gyro drift estimate rad/s
- float omegaIy; ///< Y gyro drift estimate rad/s
- float omegaIz; ///< Z gyro drift estimate rad/s
- float accel_weight; ///< average accel_weight
- float renorm_val; ///< average renormalisation value
- float error_rp; ///< average error_roll_pitch value
- float error_yaw; ///< average error_yaw value
-} mavlink_ahrs_t;
-
-#define MAVLINK_MSG_ID_AHRS_LEN 28
-#define MAVLINK_MSG_ID_163_LEN 28
-
-#define MAVLINK_MSG_ID_AHRS_CRC 127
-#define MAVLINK_MSG_ID_163_CRC 127
-
-
-
-#define MAVLINK_MESSAGE_INFO_AHRS { \
-	"AHRS", \
-	7, \
-	{  { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \
-         { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \
-         { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \
-         { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \
-         { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \
-         { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \
-         { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a ahrs 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 omegaIx X gyro drift estimate rad/s
- * @param omegaIy Y gyro drift estimate rad/s
- * @param omegaIz Z gyro drift estimate rad/s
- * @param accel_weight average accel_weight
- * @param renorm_val average renormalisation value
- * @param error_rp average error_roll_pitch value
- * @param error_yaw average error_yaw value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_AHRS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN);
-#else
-	mavlink_ahrs_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AHRS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a ahrs 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 omegaIx X gyro drift estimate rad/s
- * @param omegaIy Y gyro drift estimate rad/s
- * @param omegaIz Z gyro drift estimate rad/s
- * @param accel_weight average accel_weight
- * @param renorm_val average renormalisation value
- * @param error_rp average error_roll_pitch value
- * @param error_yaw average error_yaw value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_AHRS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN);
-#else
-	mavlink_ahrs_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AHRS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a ahrs 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 ahrs C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
-{
-	return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
-}
-
-/**
- * @brief Encode a ahrs 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 ahrs C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ahrs_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
-{
-	return mavlink_msg_ahrs_pack_chan(system_id, component_id, chan, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
-}
-
-/**
- * @brief Send a ahrs message
- * @param chan MAVLink channel to send the message
- *
- * @param omegaIx X gyro drift estimate rad/s
- * @param omegaIy Y gyro drift estimate rad/s
- * @param omegaIz Z gyro drift estimate rad/s
- * @param accel_weight average accel_weight
- * @param renorm_val average renormalisation value
- * @param error_rp average error_roll_pitch value
- * @param error_yaw average error_yaw value
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_ahrs_send(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[MAVLINK_MSG_ID_AHRS_LEN];
-	_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;
-	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
-
-// MESSAGE AHRS UNPACKING
-
-
-/**
- * @brief Get field omegaIx from ahrs message
- *
- * @return X gyro drift estimate rad/s
- */
-static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field omegaIy from ahrs message
- *
- * @return Y gyro drift estimate rad/s
- */
-static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field omegaIz from ahrs message
- *
- * @return Z gyro drift estimate rad/s
- */
-static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field accel_weight from ahrs message
- *
- * @return average accel_weight
- */
-static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field renorm_val from ahrs message
- *
- * @return average renormalisation value
- */
-static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field error_rp from ahrs message
- *
- * @return average error_roll_pitch value
- */
-static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field error_yaw from ahrs message
- *
- * @return average error_yaw value
- */
-static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a ahrs message into a struct
- *
- * @param msg The message to decode
- * @param ahrs C-struct to decode the message contents into
- */
-static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg);
-	ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg);
-	ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg);
-	ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg);
-	ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg);
-	ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg);
-	ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg);
-#else
-	memcpy(ahrs, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h
deleted file mode 100644
index f6fde9590..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE AHRS2 PACKING
-
-#define MAVLINK_MSG_ID_AHRS2 178
-
-typedef struct __mavlink_ahrs2_t
-{
- float roll; ///< Roll angle (rad)
- float pitch; ///< Pitch angle (rad)
- float yaw; ///< Yaw angle (rad)
- float altitude; ///< Altitude (MSL)
- int32_t lat; ///< Latitude in degrees * 1E7
- int32_t lng; ///< Longitude in degrees * 1E7
-} mavlink_ahrs2_t;
-
-#define MAVLINK_MSG_ID_AHRS2_LEN 24
-#define MAVLINK_MSG_ID_178_LEN 24
-
-#define MAVLINK_MSG_ID_AHRS2_CRC 47
-#define MAVLINK_MSG_ID_178_CRC 47
-
-
-
-#define MAVLINK_MESSAGE_INFO_AHRS2 { \
-	"AHRS2", \
-	6, \
-	{  { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs2_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs2_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs2_t, yaw) }, \
-         { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs2_t, altitude) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs2_t, lat) }, \
-         { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs2_t, lng) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a ahrs2 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 roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param altitude Altitude (MSL)
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ahrs2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AHRS2_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN);
-#else
-	mavlink_ahrs2_t packet;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.altitude = altitude;
-	packet.lat = lat;
-	packet.lng = lng;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AHRS2;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN);
-#endif
-}
-
-/**
- * @brief Pack a ahrs2 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 roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param altitude Altitude (MSL)
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ahrs2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AHRS2_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN);
-#else
-	mavlink_ahrs2_t packet;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.altitude = altitude;
-	packet.lat = lat;
-	packet.lng = lng;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AHRS2;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN);
-#endif
-}
-
-/**
- * @brief Encode a ahrs2 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 ahrs2 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ahrs2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2)
-{
-	return mavlink_msg_ahrs2_pack(system_id, component_id, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng);
-}
-
-/**
- * @brief Encode a ahrs2 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 ahrs2 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ahrs2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2)
-{
-	return mavlink_msg_ahrs2_pack_chan(system_id, component_id, chan, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng);
-}
-
-/**
- * @brief Send a ahrs2 message
- * @param chan MAVLink channel to send the message
- *
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param altitude Altitude (MSL)
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_ahrs2_send(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[MAVLINK_MSG_ID_AHRS2_LEN];
-	_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;
-	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
-
-// MESSAGE AHRS2 UNPACKING
-
-
-/**
- * @brief Get field roll from ahrs2 message
- *
- * @return Roll angle (rad)
- */
-static inline float mavlink_msg_ahrs2_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field pitch from ahrs2 message
- *
- * @return Pitch angle (rad)
- */
-static inline float mavlink_msg_ahrs2_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field yaw from ahrs2 message
- *
- * @return Yaw angle (rad)
- */
-static inline float mavlink_msg_ahrs2_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field altitude from ahrs2 message
- *
- * @return Altitude (MSL)
- */
-static inline float mavlink_msg_ahrs2_get_altitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field lat from ahrs2 message
- *
- * @return Latitude in degrees * 1E7
- */
-static inline int32_t mavlink_msg_ahrs2_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  16);
-}
-
-/**
- * @brief Get field lng from ahrs2 message
- *
- * @return Longitude in degrees * 1E7
- */
-static inline int32_t mavlink_msg_ahrs2_get_lng(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  20);
-}
-
-/**
- * @brief Decode a ahrs2 message into a struct
- *
- * @param msg The message to decode
- * @param ahrs2 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_ahrs2_decode(const mavlink_message_t* msg, mavlink_ahrs2_t* ahrs2)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	ahrs2->roll = mavlink_msg_ahrs2_get_roll(msg);
-	ahrs2->pitch = mavlink_msg_ahrs2_get_pitch(msg);
-	ahrs2->yaw = mavlink_msg_ahrs2_get_yaw(msg);
-	ahrs2->altitude = mavlink_msg_ahrs2_get_altitude(msg);
-	ahrs2->lat = mavlink_msg_ahrs2_get_lat(msg);
-	ahrs2->lng = mavlink_msg_ahrs2_get_lng(msg);
-#else
-	memcpy(ahrs2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS2_LEN);
-#endif
-}
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
deleted file mode 100644
index d046f2ad0..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h
+++ /dev/null
@@ -1,419 +0,0 @@
-// MESSAGE AIRSPEED_AUTOCAL PACKING
-
-#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL 174
-
-typedef struct __mavlink_airspeed_autocal_t
-{
- float vx; ///< GPS velocity north m/s
- float vy; ///< GPS velocity east m/s
- float vz; ///< GPS velocity down m/s
- float diff_pressure; ///< Differential pressure pascals
- float EAS2TAS; ///< Estimated to true airspeed ratio
- float ratio; ///< Airspeed ratio
- float state_x; ///< EKF state x
- float state_y; ///< EKF state y
- float state_z; ///< EKF state z
- float Pax; ///< EKF Pax
- float Pby; ///< EKF Pby
- float Pcz; ///< EKF Pcz
-} mavlink_airspeed_autocal_t;
-
-#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN 48
-#define MAVLINK_MSG_ID_174_LEN 48
-
-#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC 167
-#define MAVLINK_MSG_ID_174_CRC 167
-
-
-
-#define MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL { \
-	"AIRSPEED_AUTOCAL", \
-	12, \
-	{  { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_airspeed_autocal_t, vx) }, \
-         { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_airspeed_autocal_t, vy) }, \
-         { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_airspeed_autocal_t, vz) }, \
-         { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_airspeed_autocal_t, diff_pressure) }, \
-         { "EAS2TAS", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_airspeed_autocal_t, EAS2TAS) }, \
-         { "ratio", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_airspeed_autocal_t, ratio) }, \
-         { "state_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_airspeed_autocal_t, state_x) }, \
-         { "state_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_airspeed_autocal_t, state_y) }, \
-         { "state_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_airspeed_autocal_t, state_z) }, \
-         { "Pax", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_airspeed_autocal_t, Pax) }, \
-         { "Pby", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_airspeed_autocal_t, Pby) }, \
-         { "Pcz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_airspeed_autocal_t, Pcz) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a airspeed_autocal 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 vx GPS velocity north m/s
- * @param vy GPS velocity east m/s
- * @param vz GPS velocity down m/s
- * @param diff_pressure Differential pressure pascals
- * @param EAS2TAS Estimated to true airspeed ratio
- * @param ratio Airspeed ratio
- * @param state_x EKF state x
- * @param state_y EKF state y
- * @param state_z EKF state z
- * @param Pax EKF Pax
- * @param Pby EKF Pby
- * @param Pcz EKF Pcz
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_airspeed_autocal_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
-#else
-	mavlink_airspeed_autocal_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a airspeed_autocal 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 vx GPS velocity north m/s
- * @param vy GPS velocity east m/s
- * @param vz GPS velocity down m/s
- * @param diff_pressure Differential pressure pascals
- * @param EAS2TAS Estimated to true airspeed ratio
- * @param ratio Airspeed ratio
- * @param state_x EKF state x
- * @param state_y EKF state y
- * @param state_z EKF state z
- * @param Pax EKF Pax
- * @param Pby EKF Pby
- * @param Pcz EKF Pcz
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_airspeed_autocal_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
-#else
-	mavlink_airspeed_autocal_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a airspeed_autocal 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 airspeed_autocal C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_airspeed_autocal_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal)
-{
-	return mavlink_msg_airspeed_autocal_pack(system_id, component_id, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz);
-}
-
-/**
- * @brief Encode a airspeed_autocal 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 airspeed_autocal C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_airspeed_autocal_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal)
-{
-	return mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, chan, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz);
-}
-
-/**
- * @brief Send a airspeed_autocal message
- * @param chan MAVLink channel to send the message
- *
- * @param vx GPS velocity north m/s
- * @param vy GPS velocity east m/s
- * @param vz GPS velocity down m/s
- * @param diff_pressure Differential pressure pascals
- * @param EAS2TAS Estimated to true airspeed ratio
- * @param ratio Airspeed ratio
- * @param state_x EKF state x
- * @param state_y EKF state y
- * @param state_z EKF state z
- * @param Pax EKF Pax
- * @param Pby EKF Pby
- * @param Pcz EKF Pcz
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_airspeed_autocal_send(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[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN];
-	_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;
-	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
-
-// MESSAGE AIRSPEED_AUTOCAL UNPACKING
-
-
-/**
- * @brief Get field vx from airspeed_autocal message
- *
- * @return GPS velocity north m/s
- */
-static inline float mavlink_msg_airspeed_autocal_get_vx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field vy from airspeed_autocal message
- *
- * @return GPS velocity east m/s
- */
-static inline float mavlink_msg_airspeed_autocal_get_vy(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field vz from airspeed_autocal message
- *
- * @return GPS velocity down m/s
- */
-static inline float mavlink_msg_airspeed_autocal_get_vz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field diff_pressure from airspeed_autocal message
- *
- * @return Differential pressure pascals
- */
-static inline float mavlink_msg_airspeed_autocal_get_diff_pressure(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field EAS2TAS from airspeed_autocal message
- *
- * @return Estimated to true airspeed ratio
- */
-static inline float mavlink_msg_airspeed_autocal_get_EAS2TAS(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field ratio from airspeed_autocal message
- *
- * @return Airspeed ratio
- */
-static inline float mavlink_msg_airspeed_autocal_get_ratio(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field state_x from airspeed_autocal message
- *
- * @return EKF state x
- */
-static inline float mavlink_msg_airspeed_autocal_get_state_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field state_y from airspeed_autocal message
- *
- * @return EKF state y
- */
-static inline float mavlink_msg_airspeed_autocal_get_state_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field state_z from airspeed_autocal message
- *
- * @return EKF state z
- */
-static inline float mavlink_msg_airspeed_autocal_get_state_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field Pax from airspeed_autocal message
- *
- * @return EKF Pax
- */
-static inline float mavlink_msg_airspeed_autocal_get_Pax(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  36);
-}
-
-/**
- * @brief Get field Pby from airspeed_autocal message
- *
- * @return EKF Pby
- */
-static inline float mavlink_msg_airspeed_autocal_get_Pby(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  40);
-}
-
-/**
- * @brief Get field Pcz from airspeed_autocal message
- *
- * @return EKF Pcz
- */
-static inline float mavlink_msg_airspeed_autocal_get_Pcz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  44);
-}
-
-/**
- * @brief Decode a airspeed_autocal message into a struct
- *
- * @param msg The message to decode
- * @param airspeed_autocal C-struct to decode the message contents into
- */
-static inline void mavlink_msg_airspeed_autocal_decode(const mavlink_message_t* msg, mavlink_airspeed_autocal_t* airspeed_autocal)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	airspeed_autocal->vx = mavlink_msg_airspeed_autocal_get_vx(msg);
-	airspeed_autocal->vy = mavlink_msg_airspeed_autocal_get_vy(msg);
-	airspeed_autocal->vz = mavlink_msg_airspeed_autocal_get_vz(msg);
-	airspeed_autocal->diff_pressure = mavlink_msg_airspeed_autocal_get_diff_pressure(msg);
-	airspeed_autocal->EAS2TAS = mavlink_msg_airspeed_autocal_get_EAS2TAS(msg);
-	airspeed_autocal->ratio = mavlink_msg_airspeed_autocal_get_ratio(msg);
-	airspeed_autocal->state_x = mavlink_msg_airspeed_autocal_get_state_x(msg);
-	airspeed_autocal->state_y = mavlink_msg_airspeed_autocal_get_state_y(msg);
-	airspeed_autocal->state_z = mavlink_msg_airspeed_autocal_get_state_z(msg);
-	airspeed_autocal->Pax = mavlink_msg_airspeed_autocal_get_Pax(msg);
-	airspeed_autocal->Pby = mavlink_msg_airspeed_autocal_get_Pby(msg);
-	airspeed_autocal->Pcz = mavlink_msg_airspeed_autocal_get_Pcz(msg);
-#else
-	memcpy(airspeed_autocal, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
-#endif
-}
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
deleted file mode 100644
index 821ce73e4..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE AP_ADC PACKING
-
-#define MAVLINK_MSG_ID_AP_ADC 153
-
-typedef struct __mavlink_ap_adc_t
-{
- uint16_t adc1; ///< ADC output 1
- uint16_t adc2; ///< ADC output 2
- uint16_t adc3; ///< ADC output 3
- uint16_t adc4; ///< ADC output 4
- uint16_t adc5; ///< ADC output 5
- uint16_t adc6; ///< ADC output 6
-} mavlink_ap_adc_t;
-
-#define MAVLINK_MSG_ID_AP_ADC_LEN 12
-#define MAVLINK_MSG_ID_153_LEN 12
-
-#define MAVLINK_MSG_ID_AP_ADC_CRC 188
-#define MAVLINK_MSG_ID_153_CRC 188
-
-
-
-#define MAVLINK_MESSAGE_INFO_AP_ADC { \
-	"AP_ADC", \
-	6, \
-	{  { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \
-         { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \
-         { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \
-         { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \
-         { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \
-         { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a ap_adc 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 adc1 ADC output 1
- * @param adc2 ADC output 2
- * @param adc3 ADC output 3
- * @param adc4 ADC output 4
- * @param adc5 ADC output 5
- * @param adc6 ADC output 6
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_AP_ADC_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN);
-#else
-	mavlink_ap_adc_t packet;
-	packet.adc1 = adc1;
-	packet.adc2 = adc2;
-	packet.adc3 = adc3;
-	packet.adc4 = adc4;
-	packet.adc5 = adc5;
-	packet.adc6 = adc6;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AP_ADC;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-}
-
-/**
- * @brief Pack a ap_adc 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 adc1 ADC output 1
- * @param adc2 ADC output 2
- * @param adc3 ADC output 3
- * @param adc4 ADC output 4
- * @param adc5 ADC output 5
- * @param adc6 ADC output 6
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_AP_ADC_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN);
-#else
-	mavlink_ap_adc_t packet;
-	packet.adc1 = adc1;
-	packet.adc2 = adc2;
-	packet.adc3 = adc3;
-	packet.adc4 = adc4;
-	packet.adc5 = adc5;
-	packet.adc6 = adc6;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AP_ADC;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-}
-
-/**
- * @brief Encode a ap_adc 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 ap_adc C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc)
-{
-	return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6);
-}
-
-/**
- * @brief Encode a ap_adc 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 ap_adc C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ap_adc_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc)
-{
-	return mavlink_msg_ap_adc_pack_chan(system_id, component_id, chan, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6);
-}
-
-/**
- * @brief Send a ap_adc message
- * @param chan MAVLink channel to send the message
- *
- * @param adc1 ADC output 1
- * @param adc2 ADC output 2
- * @param adc3 ADC output 3
- * @param adc4 ADC output 4
- * @param adc5 ADC output 5
- * @param adc6 ADC output 6
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_ap_adc_send(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[MAVLINK_MSG_ID_AP_ADC_LEN];
-	_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;
-	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
-
-// MESSAGE AP_ADC UNPACKING
-
-
-/**
- * @brief Get field adc1 from ap_adc message
- *
- * @return ADC output 1
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field adc2 from ap_adc message
- *
- * @return ADC output 2
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Get field adc3 from ap_adc message
- *
- * @return ADC output 3
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field adc4 from ap_adc message
- *
- * @return ADC output 4
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field adc5 from ap_adc message
- *
- * @return ADC output 5
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field adc6 from ap_adc message
- *
- * @return ADC output 6
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Decode a ap_adc message into a struct
- *
- * @param msg The message to decode
- * @param ap_adc C-struct to decode the message contents into
- */
-static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg);
-	ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg);
-	ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg);
-	ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg);
-	ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg);
-	ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg);
-#else
-	memcpy(ap_adc, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AP_ADC_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
deleted file mode 100644
index 9200eefa0..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h
+++ /dev/null
@@ -1,215 +0,0 @@
-// MESSAGE DATA16 PACKING
-
-#define MAVLINK_MSG_ID_DATA16 169
-
-typedef struct __mavlink_data16_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[16]; ///< raw data
-} mavlink_data16_t;
-
-#define MAVLINK_MSG_ID_DATA16_LEN 18
-#define MAVLINK_MSG_ID_169_LEN 18
-
-#define MAVLINK_MSG_ID_DATA16_CRC 234
-#define MAVLINK_MSG_ID_169_CRC 234
-
-#define MAVLINK_MSG_DATA16_FIELD_DATA_LEN 16
-
-#define MAVLINK_MESSAGE_INFO_DATA16 { \
-	"DATA16", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data16_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data16_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 16, 2, offsetof(mavlink_data16_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data16 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 type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA16_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 16);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN);
-#else
-	mavlink_data16_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA16;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data16 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 type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA16_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 16);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN);
-#else
-	mavlink_data16_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA16;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data16 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 data16 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data16_t* data16)
-{
-	return mavlink_msg_data16_pack(system_id, component_id, msg, data16->type, data16->len, data16->data);
-}
-
-/**
- * @brief Encode a data16 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 data16 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data16_t* data16)
-{
-	return mavlink_msg_data16_pack_chan(system_id, component_id, chan, msg, data16->type, data16->len, data16->data);
-}
-
-/**
- * @brief Send a data16 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA16_LEN];
-	_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;
-	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
-
-// MESSAGE DATA16 UNPACKING
-
-
-/**
- * @brief Get field type from data16 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data16_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data16 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data16_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data16 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data16_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 16,  2);
-}
-
-/**
- * @brief Decode a data16 message into a struct
- *
- * @param msg The message to decode
- * @param data16 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data16_decode(const mavlink_message_t* msg, mavlink_data16_t* data16)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data16->type = mavlink_msg_data16_get_type(msg);
-	data16->len = mavlink_msg_data16_get_len(msg);
-	mavlink_msg_data16_get_data(msg, data16->data);
-#else
-	memcpy(data16, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h
deleted file mode 100644
index 3afedb787..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h
+++ /dev/null
@@ -1,215 +0,0 @@
-// MESSAGE DATA32 PACKING
-
-#define MAVLINK_MSG_ID_DATA32 170
-
-typedef struct __mavlink_data32_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[32]; ///< raw data
-} mavlink_data32_t;
-
-#define MAVLINK_MSG_ID_DATA32_LEN 34
-#define MAVLINK_MSG_ID_170_LEN 34
-
-#define MAVLINK_MSG_ID_DATA32_CRC 73
-#define MAVLINK_MSG_ID_170_CRC 73
-
-#define MAVLINK_MSG_DATA32_FIELD_DATA_LEN 32
-
-#define MAVLINK_MESSAGE_INFO_DATA32 { \
-	"DATA32", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data32_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data32_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 32, 2, offsetof(mavlink_data32_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data32 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 type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA32_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN);
-#else
-	mavlink_data32_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA32;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data32 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 type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA32_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN);
-#else
-	mavlink_data32_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA32;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data32 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 data32 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data32_t* data32)
-{
-	return mavlink_msg_data32_pack(system_id, component_id, msg, data32->type, data32->len, data32->data);
-}
-
-/**
- * @brief Encode a data32 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 data32 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data32_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data32_t* data32)
-{
-	return mavlink_msg_data32_pack_chan(system_id, component_id, chan, msg, data32->type, data32->len, data32->data);
-}
-
-/**
- * @brief Send a data32 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA32_LEN];
-	_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;
-	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
-
-// MESSAGE DATA32 UNPACKING
-
-
-/**
- * @brief Get field type from data32 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data32_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data32 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data32_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data32 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data32_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 32,  2);
-}
-
-/**
- * @brief Decode a data32 message into a struct
- *
- * @param msg The message to decode
- * @param data32 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data32_decode(const mavlink_message_t* msg, mavlink_data32_t* data32)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data32->type = mavlink_msg_data32_get_type(msg);
-	data32->len = mavlink_msg_data32_get_len(msg);
-	mavlink_msg_data32_get_data(msg, data32->data);
-#else
-	memcpy(data32, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h
deleted file mode 100644
index 6931ada16..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h
+++ /dev/null
@@ -1,215 +0,0 @@
-// MESSAGE DATA64 PACKING
-
-#define MAVLINK_MSG_ID_DATA64 171
-
-typedef struct __mavlink_data64_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[64]; ///< raw data
-} mavlink_data64_t;
-
-#define MAVLINK_MSG_ID_DATA64_LEN 66
-#define MAVLINK_MSG_ID_171_LEN 66
-
-#define MAVLINK_MSG_ID_DATA64_CRC 181
-#define MAVLINK_MSG_ID_171_CRC 181
-
-#define MAVLINK_MSG_DATA64_FIELD_DATA_LEN 64
-
-#define MAVLINK_MESSAGE_INFO_DATA64 { \
-	"DATA64", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data64_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data64_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 2, offsetof(mavlink_data64_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data64 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 type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA64_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 64);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN);
-#else
-	mavlink_data64_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA64;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data64 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 type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA64_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 64);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN);
-#else
-	mavlink_data64_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA64;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data64 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 data64 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data64_t* data64)
-{
-	return mavlink_msg_data64_pack(system_id, component_id, msg, data64->type, data64->len, data64->data);
-}
-
-/**
- * @brief Encode a data64 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 data64 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data64_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data64_t* data64)
-{
-	return mavlink_msg_data64_pack_chan(system_id, component_id, chan, msg, data64->type, data64->len, data64->data);
-}
-
-/**
- * @brief Send a data64 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA64_LEN];
-	_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;
-	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
-
-// MESSAGE DATA64 UNPACKING
-
-
-/**
- * @brief Get field type from data64 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data64_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data64 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data64_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data64 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data64_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 64,  2);
-}
-
-/**
- * @brief Decode a data64 message into a struct
- *
- * @param msg The message to decode
- * @param data64 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data64_decode(const mavlink_message_t* msg, mavlink_data64_t* data64)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data64->type = mavlink_msg_data64_get_type(msg);
-	data64->len = mavlink_msg_data64_get_len(msg);
-	mavlink_msg_data64_get_data(msg, data64->data);
-#else
-	memcpy(data64, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h
deleted file mode 100644
index cffc7d7e7..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h
+++ /dev/null
@@ -1,215 +0,0 @@
-// MESSAGE DATA96 PACKING
-
-#define MAVLINK_MSG_ID_DATA96 172
-
-typedef struct __mavlink_data96_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[96]; ///< raw data
-} mavlink_data96_t;
-
-#define MAVLINK_MSG_ID_DATA96_LEN 98
-#define MAVLINK_MSG_ID_172_LEN 98
-
-#define MAVLINK_MSG_ID_DATA96_CRC 22
-#define MAVLINK_MSG_ID_172_CRC 22
-
-#define MAVLINK_MSG_DATA96_FIELD_DATA_LEN 96
-
-#define MAVLINK_MESSAGE_INFO_DATA96 { \
-	"DATA96", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data96_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data96_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 96, 2, offsetof(mavlink_data96_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data96 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 type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA96_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 96);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN);
-#else
-	mavlink_data96_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA96;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data96 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 type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA96_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 96);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN);
-#else
-	mavlink_data96_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA96;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data96 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 data96 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data96_t* data96)
-{
-	return mavlink_msg_data96_pack(system_id, component_id, msg, data96->type, data96->len, data96->data);
-}
-
-/**
- * @brief Encode a data96 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 data96 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data96_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data96_t* data96)
-{
-	return mavlink_msg_data96_pack_chan(system_id, component_id, chan, msg, data96->type, data96->len, data96->data);
-}
-
-/**
- * @brief Send a data96 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA96_LEN];
-	_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;
-	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
-
-// MESSAGE DATA96 UNPACKING
-
-
-/**
- * @brief Get field type from data96 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data96_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data96 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data96_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data96 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data96_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 96,  2);
-}
-
-/**
- * @brief Decode a data96 message into a struct
- *
- * @param msg The message to decode
- * @param data96 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data96_decode(const mavlink_message_t* msg, mavlink_data96_t* data96)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data96->type = mavlink_msg_data96_get_type(msg);
-	data96->len = mavlink_msg_data96_get_len(msg);
-	mavlink_msg_data96_get_data(msg, data96->data);
-#else
-	memcpy(data96, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-}
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
deleted file mode 100644
index c6518c419..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h
+++ /dev/null
@@ -1,397 +0,0 @@
-// MESSAGE DIGICAM_CONFIGURE PACKING
-
-#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE 154
-
-typedef struct __mavlink_digicam_configure_t
-{
- float extra_value; ///< Correspondent value to given extra_param
- uint16_t shutter_speed; ///< Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t mode; ///< Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- uint8_t aperture; ///< F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- uint8_t iso; ///< ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- uint8_t exposure_type; ///< Exposure type enumeration from 1 to N (0 means ignore)
- uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- uint8_t engine_cut_off; ///< Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore)
-} mavlink_digicam_configure_t;
-
-#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN 15
-#define MAVLINK_MSG_ID_154_LEN 15
-
-#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC 84
-#define MAVLINK_MSG_ID_154_CRC 84
-
-
-
-#define MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE { \
-	"DIGICAM_CONFIGURE", \
-	11, \
-	{  { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_configure_t, extra_value) }, \
-         { "shutter_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_digicam_configure_t, shutter_speed) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_configure_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_configure_t, target_component) }, \
-         { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_configure_t, mode) }, \
-         { "aperture", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_configure_t, aperture) }, \
-         { "iso", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_configure_t, iso) }, \
-         { "exposure_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_configure_t, exposure_type) }, \
-         { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_configure_t, command_id) }, \
-         { "engine_cut_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_digicam_configure_t, engine_cut_off) }, \
-         { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_digicam_configure_t, extra_param) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a digicam_configure 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 target_system System ID
- * @param target_component Component ID
- * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#else
-	mavlink_digicam_configure_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a digicam_configure 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 target_system System ID
- * @param target_component Component ID
- * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#else
-	mavlink_digicam_configure_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a digicam_configure 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 digicam_configure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure)
-{
-	return mavlink_msg_digicam_configure_pack(system_id, component_id, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value);
-}
-
-/**
- * @brief Encode a digicam_configure 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 digicam_configure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_digicam_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure)
-{
-	return mavlink_msg_digicam_configure_pack_chan(system_id, component_id, chan, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value);
-}
-
-/**
- * @brief Send a digicam_configure message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_digicam_configure_send(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[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN];
-	_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;
-	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
-
-// MESSAGE DIGICAM_CONFIGURE UNPACKING
-
-
-/**
- * @brief Get field target_system from digicam_configure message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field target_component from digicam_configure message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field mode from digicam_configure message
- *
- * @return Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field shutter_speed from digicam_configure message
- *
- * @return Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- */
-static inline uint16_t mavlink_msg_digicam_configure_get_shutter_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field aperture from digicam_configure message
- *
- * @return F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_aperture(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Get field iso from digicam_configure message
- *
- * @return ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_iso(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field exposure_type from digicam_configure message
- *
- * @return Exposure type enumeration from 1 to N (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_exposure_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field command_id from digicam_configure message
- *
- * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_command_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field engine_cut_off from digicam_configure message
- *
- * @return Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_engine_cut_off(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  13);
-}
-
-/**
- * @brief Get field extra_param from digicam_configure message
- *
- * @return Extra parameters enumeration (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_extra_param(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Get field extra_value from digicam_configure message
- *
- * @return Correspondent value to given extra_param
- */
-static inline float mavlink_msg_digicam_configure_get_extra_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Decode a digicam_configure message into a struct
- *
- * @param msg The message to decode
- * @param digicam_configure C-struct to decode the message contents into
- */
-static inline void mavlink_msg_digicam_configure_decode(const mavlink_message_t* msg, mavlink_digicam_configure_t* digicam_configure)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	digicam_configure->extra_value = mavlink_msg_digicam_configure_get_extra_value(msg);
-	digicam_configure->shutter_speed = mavlink_msg_digicam_configure_get_shutter_speed(msg);
-	digicam_configure->target_system = mavlink_msg_digicam_configure_get_target_system(msg);
-	digicam_configure->target_component = mavlink_msg_digicam_configure_get_target_component(msg);
-	digicam_configure->mode = mavlink_msg_digicam_configure_get_mode(msg);
-	digicam_configure->aperture = mavlink_msg_digicam_configure_get_aperture(msg);
-	digicam_configure->iso = mavlink_msg_digicam_configure_get_iso(msg);
-	digicam_configure->exposure_type = mavlink_msg_digicam_configure_get_exposure_type(msg);
-	digicam_configure->command_id = mavlink_msg_digicam_configure_get_command_id(msg);
-	digicam_configure->engine_cut_off = mavlink_msg_digicam_configure_get_engine_cut_off(msg);
-	digicam_configure->extra_param = mavlink_msg_digicam_configure_get_extra_param(msg);
-#else
-	memcpy(digicam_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-}
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
deleted file mode 100644
index bfa5414a3..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE DIGICAM_CONTROL PACKING
-
-#define MAVLINK_MSG_ID_DIGICAM_CONTROL 155
-
-typedef struct __mavlink_digicam_control_t
-{
- float extra_value; ///< Correspondent value to given extra_param
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t session; ///< 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- uint8_t zoom_pos; ///< 1 to N //Zoom's absolute position (0 means ignore)
- int8_t zoom_step; ///< -100 to 100 //Zooming step value to offset zoom from the current position
- uint8_t focus_lock; ///< 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- uint8_t shot; ///< 0: ignore, 1: shot or start filming
- uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore)
-} mavlink_digicam_control_t;
-
-#define MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN 13
-#define MAVLINK_MSG_ID_155_LEN 13
-
-#define MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC 22
-#define MAVLINK_MSG_ID_155_CRC 22
-
-
-
-#define MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL { \
-	"DIGICAM_CONTROL", \
-	10, \
-	{  { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_control_t, extra_value) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_digicam_control_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_control_t, target_component) }, \
-         { "session", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_control_t, session) }, \
-         { "zoom_pos", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_control_t, zoom_pos) }, \
-         { "zoom_step", NULL, MAVLINK_TYPE_INT8_T, 0, 8, offsetof(mavlink_digicam_control_t, zoom_step) }, \
-         { "focus_lock", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_control_t, focus_lock) }, \
-         { "shot", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_control_t, shot) }, \
-         { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_control_t, command_id) }, \
-         { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_control_t, extra_param) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a digicam_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 target_system System ID
- * @param target_component Component ID
- * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)
- * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position
- * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- * @param shot 0: ignore, 1: shot or start filming
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#else
-	mavlink_digicam_control_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a digicam_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 target_system System ID
- * @param target_component Component ID
- * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)
- * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position
- * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- * @param shot 0: ignore, 1: shot or start filming
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#else
-	mavlink_digicam_control_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a digicam_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 digicam_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control)
-{
-	return mavlink_msg_digicam_control_pack(system_id, component_id, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value);
-}
-
-/**
- * @brief Encode a digicam_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 digicam_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_digicam_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control)
-{
-	return mavlink_msg_digicam_control_pack_chan(system_id, component_id, chan, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value);
-}
-
-/**
- * @brief Send a digicam_control message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)
- * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position
- * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- * @param shot 0: ignore, 1: shot or start filming
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_digicam_control_send(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[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN];
-	_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;
-	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
-
-// MESSAGE DIGICAM_CONTROL UNPACKING
-
-
-/**
- * @brief Get field target_system from digicam_control message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_digicam_control_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field target_component from digicam_control message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_digicam_control_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field session from digicam_control message
- *
- * @return 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- */
-static inline uint8_t mavlink_msg_digicam_control_get_session(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field zoom_pos from digicam_control message
- *
- * @return 1 to N //Zoom's absolute position (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_control_get_zoom_pos(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field zoom_step from digicam_control message
- *
- * @return -100 to 100 //Zooming step value to offset zoom from the current position
- */
-static inline int8_t mavlink_msg_digicam_control_get_zoom_step(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int8_t(msg,  8);
-}
-
-/**
- * @brief Get field focus_lock from digicam_control message
- *
- * @return 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- */
-static inline uint8_t mavlink_msg_digicam_control_get_focus_lock(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Get field shot from digicam_control message
- *
- * @return 0: ignore, 1: shot or start filming
- */
-static inline uint8_t mavlink_msg_digicam_control_get_shot(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field command_id from digicam_control message
- *
- * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- */
-static inline uint8_t mavlink_msg_digicam_control_get_command_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field extra_param from digicam_control message
- *
- * @return Extra parameters enumeration (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_control_get_extra_param(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field extra_value from digicam_control message
- *
- * @return Correspondent value to given extra_param
- */
-static inline float mavlink_msg_digicam_control_get_extra_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Decode a digicam_control message into a struct
- *
- * @param msg The message to decode
- * @param digicam_control C-struct to decode the message contents into
- */
-static inline void mavlink_msg_digicam_control_decode(const mavlink_message_t* msg, mavlink_digicam_control_t* digicam_control)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	digicam_control->extra_value = mavlink_msg_digicam_control_get_extra_value(msg);
-	digicam_control->target_system = mavlink_msg_digicam_control_get_target_system(msg);
-	digicam_control->target_component = mavlink_msg_digicam_control_get_target_component(msg);
-	digicam_control->session = mavlink_msg_digicam_control_get_session(msg);
-	digicam_control->zoom_pos = mavlink_msg_digicam_control_get_zoom_pos(msg);
-	digicam_control->zoom_step = mavlink_msg_digicam_control_get_zoom_step(msg);
-	digicam_control->focus_lock = mavlink_msg_digicam_control_get_focus_lock(msg);
-	digicam_control->shot = mavlink_msg_digicam_control_get_shot(msg);
-	digicam_control->command_id = mavlink_msg_digicam_control_get_command_id(msg);
-	digicam_control->extra_param = mavlink_msg_digicam_control_get_extra_param(msg);
-#else
-	memcpy(digicam_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-}
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
deleted file mode 100644
index fe3677d53..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE FENCE_FETCH_POINT PACKING
-
-#define MAVLINK_MSG_ID_FENCE_FETCH_POINT 161
-
-typedef struct __mavlink_fence_fetch_point_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t idx; ///< point index (first point is 1, 0 is for return point)
-} mavlink_fence_fetch_point_t;
-
-#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN 3
-#define MAVLINK_MSG_ID_161_LEN 3
-
-#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC 68
-#define MAVLINK_MSG_ID_161_CRC 68
-
-
-
-#define MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT { \
-	"FENCE_FETCH_POINT", \
-	3, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fence_fetch_point_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_fence_fetch_point_t, target_component) }, \
-         { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_fence_fetch_point_t, idx) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a fence_fetch_point 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 target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t idx)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, idx);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#else
-	mavlink_fence_fetch_point_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a fence_fetch_point 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 target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t idx)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, idx);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#else
-	mavlink_fence_fetch_point_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a fence_fetch_point 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 fence_fetch_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point)
-{
-	return mavlink_msg_fence_fetch_point_pack(system_id, component_id, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx);
-}
-
-/**
- * @brief Encode a fence_fetch_point 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 fence_fetch_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point)
-{
-	return mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, chan, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx);
-}
-
-/**
- * @brief Send a fence_fetch_point message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_fence_fetch_point_send(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[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN];
-	_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;
-	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
-
-// MESSAGE FENCE_FETCH_POINT UNPACKING
-
-
-/**
- * @brief Get field target_system from fence_fetch_point message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_fence_fetch_point_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from fence_fetch_point message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_fence_fetch_point_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field idx from fence_fetch_point message
- *
- * @return point index (first point is 1, 0 is for return point)
- */
-static inline uint8_t mavlink_msg_fence_fetch_point_get_idx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a fence_fetch_point message into a struct
- *
- * @param msg The message to decode
- * @param fence_fetch_point C-struct to decode the message contents into
- */
-static inline void mavlink_msg_fence_fetch_point_decode(const mavlink_message_t* msg, mavlink_fence_fetch_point_t* fence_fetch_point)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	fence_fetch_point->target_system = mavlink_msg_fence_fetch_point_get_target_system(msg);
-	fence_fetch_point->target_component = mavlink_msg_fence_fetch_point_get_target_component(msg);
-	fence_fetch_point->idx = mavlink_msg_fence_fetch_point_get_idx(msg);
-#else
-	memcpy(fence_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-}
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
deleted file mode 100644
index febda6cdc..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE FENCE_POINT PACKING
-
-#define MAVLINK_MSG_ID_FENCE_POINT 160
-
-typedef struct __mavlink_fence_point_t
-{
- float lat; ///< Latitude of point
- float lng; ///< Longitude of point
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t idx; ///< point index (first point is 1, 0 is for return point)
- uint8_t count; ///< total number of points (for sanity checking)
-} mavlink_fence_point_t;
-
-#define MAVLINK_MSG_ID_FENCE_POINT_LEN 12
-#define MAVLINK_MSG_ID_160_LEN 12
-
-#define MAVLINK_MSG_ID_FENCE_POINT_CRC 78
-#define MAVLINK_MSG_ID_160_CRC 78
-
-
-
-#define MAVLINK_MESSAGE_INFO_FENCE_POINT { \
-	"FENCE_POINT", \
-	6, \
-	{  { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_fence_point_t, lat) }, \
-         { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fence_point_t, lng) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_point_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_fence_point_t, target_component) }, \
-         { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_fence_point_t, idx) }, \
-         { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_fence_point_t, count) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a fence_point 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 target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point
- * @param lng Longitude of point
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_FENCE_POINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#else
-	mavlink_fence_point_t packet;
-	packet.lat = lat;
-	packet.lng = lng;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-	packet.count = count;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a fence_point 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 target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point
- * @param lng Longitude of point
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_FENCE_POINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#else
-	mavlink_fence_point_t packet;
-	packet.lat = lat;
-	packet.lng = lng;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-	packet.count = count;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a fence_point 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 fence_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point)
-{
-	return mavlink_msg_fence_point_pack(system_id, component_id, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng);
-}
-
-/**
- * @brief Encode a fence_point 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 fence_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point)
-{
-	return mavlink_msg_fence_point_pack_chan(system_id, component_id, chan, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng);
-}
-
-/**
- * @brief Send a fence_point message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point
- * @param lng Longitude of point
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_fence_point_send(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[MAVLINK_MSG_ID_FENCE_POINT_LEN];
-	_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;
-	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
-
-// MESSAGE FENCE_POINT UNPACKING
-
-
-/**
- * @brief Get field target_system from fence_point message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_fence_point_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field target_component from fence_point message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_fence_point_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Get field idx from fence_point message
- *
- * @return point index (first point is 1, 0 is for return point)
- */
-static inline uint8_t mavlink_msg_fence_point_get_idx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field count from fence_point message
- *
- * @return total number of points (for sanity checking)
- */
-static inline uint8_t mavlink_msg_fence_point_get_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field lat from fence_point message
- *
- * @return Latitude of point
- */
-static inline float mavlink_msg_fence_point_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field lng from fence_point message
- *
- * @return Longitude of point
- */
-static inline float mavlink_msg_fence_point_get_lng(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Decode a fence_point message into a struct
- *
- * @param msg The message to decode
- * @param fence_point C-struct to decode the message contents into
- */
-static inline void mavlink_msg_fence_point_decode(const mavlink_message_t* msg, mavlink_fence_point_t* fence_point)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	fence_point->lat = mavlink_msg_fence_point_get_lat(msg);
-	fence_point->lng = mavlink_msg_fence_point_get_lng(msg);
-	fence_point->target_system = mavlink_msg_fence_point_get_target_system(msg);
-	fence_point->target_component = mavlink_msg_fence_point_get_target_component(msg);
-	fence_point->idx = mavlink_msg_fence_point_get_idx(msg);
-	fence_point->count = mavlink_msg_fence_point_get_count(msg);
-#else
-	memcpy(fence_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-}
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
deleted file mode 100644
index 612090406..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE FENCE_STATUS PACKING
-
-#define MAVLINK_MSG_ID_FENCE_STATUS 162
-
-typedef struct __mavlink_fence_status_t
-{
- uint32_t breach_time; ///< time of last breach in milliseconds since boot
- uint16_t breach_count; ///< number of fence breaches
- uint8_t breach_status; ///< 0 if currently inside fence, 1 if outside
- uint8_t breach_type; ///< last breach type (see FENCE_BREACH_* enum)
-} mavlink_fence_status_t;
-
-#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 8
-#define MAVLINK_MSG_ID_162_LEN 8
-
-#define MAVLINK_MSG_ID_FENCE_STATUS_CRC 189
-#define MAVLINK_MSG_ID_162_CRC 189
-
-
-
-#define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \
-	"FENCE_STATUS", \
-	4, \
-	{  { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \
-         { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \
-         { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \
-         { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a fence_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 breach_status 0 if currently inside fence, 1 if outside
- * @param breach_count number of fence breaches
- * @param breach_type last breach type (see FENCE_BREACH_* enum)
- * @param breach_time time of last breach in milliseconds since boot
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_FENCE_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#else
-	mavlink_fence_status_t packet;
-	packet.breach_time = breach_time;
-	packet.breach_count = breach_count;
-	packet.breach_status = breach_status;
-	packet.breach_type = breach_type;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a fence_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 breach_status 0 if currently inside fence, 1 if outside
- * @param breach_count number of fence breaches
- * @param breach_type last breach type (see FENCE_BREACH_* enum)
- * @param breach_time time of last breach in milliseconds since boot
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_FENCE_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#else
-	mavlink_fence_status_t packet;
-	packet.breach_time = breach_time;
-	packet.breach_count = breach_count;
-	packet.breach_status = breach_status;
-	packet.breach_type = breach_type;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a fence_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 fence_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status)
-{
-	return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time);
-}
-
-/**
- * @brief Encode a fence_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 fence_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status)
-{
-	return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time);
-}
-
-/**
- * @brief Send a fence_status message
- * @param chan MAVLink channel to send the message
- *
- * @param breach_status 0 if currently inside fence, 1 if outside
- * @param breach_count number of fence breaches
- * @param breach_type last breach type (see FENCE_BREACH_* enum)
- * @param breach_time time of last breach in milliseconds since boot
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_fence_status_send(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[MAVLINK_MSG_ID_FENCE_STATUS_LEN];
-	_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;
-	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
-
-// MESSAGE FENCE_STATUS UNPACKING
-
-
-/**
- * @brief Get field breach_status from fence_status message
- *
- * @return 0 if currently inside fence, 1 if outside
- */
-static inline uint8_t mavlink_msg_fence_status_get_breach_status(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field breach_count from fence_status message
- *
- * @return number of fence breaches
- */
-static inline uint16_t mavlink_msg_fence_status_get_breach_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field breach_type from fence_status message
- *
- * @return last breach type (see FENCE_BREACH_* enum)
- */
-static inline uint8_t mavlink_msg_fence_status_get_breach_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field breach_time from fence_status message
- *
- * @return time of last breach in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_fence_status_get_breach_time(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Decode a fence_status message into a struct
- *
- * @param msg The message to decode
- * @param fence_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* msg, mavlink_fence_status_t* fence_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	fence_status->breach_time = mavlink_msg_fence_status_get_breach_time(msg);
-	fence_status->breach_count = mavlink_msg_fence_status_get_breach_count(msg);
-	fence_status->breach_status = mavlink_msg_fence_status_get_breach_status(msg);
-	fence_status->breach_type = mavlink_msg_fence_status_get_breach_type(msg);
-#else
-	memcpy(fence_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h
deleted file mode 100644
index 2f5dea513..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE HWSTATUS PACKING
-
-#define MAVLINK_MSG_ID_HWSTATUS 165
-
-typedef struct __mavlink_hwstatus_t
-{
- uint16_t Vcc; ///< board voltage (mV)
- uint8_t I2Cerr; ///< I2C error count
-} mavlink_hwstatus_t;
-
-#define MAVLINK_MSG_ID_HWSTATUS_LEN 3
-#define MAVLINK_MSG_ID_165_LEN 3
-
-#define MAVLINK_MSG_ID_HWSTATUS_CRC 21
-#define MAVLINK_MSG_ID_165_CRC 21
-
-
-
-#define MAVLINK_MESSAGE_INFO_HWSTATUS { \
-	"HWSTATUS", \
-	2, \
-	{  { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_hwstatus_t, Vcc) }, \
-         { "I2Cerr", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_hwstatus_t, I2Cerr) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hwstatus 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 board voltage (mV)
- * @param I2Cerr I2C error count
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t Vcc, uint8_t I2Cerr)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_HWSTATUS_LEN];
-	_mav_put_uint16_t(buf, 0, Vcc);
-	_mav_put_uint8_t(buf, 2, I2Cerr);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#else
-	mavlink_hwstatus_t packet;
-	packet.Vcc = Vcc;
-	packet.I2Cerr = I2Cerr;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HWSTATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hwstatus 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 board voltage (mV)
- * @param I2Cerr I2C error count
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t Vcc,uint8_t I2Cerr)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_HWSTATUS_LEN];
-	_mav_put_uint16_t(buf, 0, Vcc);
-	_mav_put_uint8_t(buf, 2, I2Cerr);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#else
-	mavlink_hwstatus_t packet;
-	packet.Vcc = Vcc;
-	packet.I2Cerr = I2Cerr;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HWSTATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hwstatus 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 hwstatus C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus)
-{
-	return mavlink_msg_hwstatus_pack(system_id, component_id, msg, hwstatus->Vcc, hwstatus->I2Cerr);
-}
-
-/**
- * @brief Encode a hwstatus 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 hwstatus C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hwstatus_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus)
-{
-	return mavlink_msg_hwstatus_pack_chan(system_id, component_id, chan, msg, hwstatus->Vcc, hwstatus->I2Cerr);
-}
-
-/**
- * @brief Send a hwstatus message
- * @param chan MAVLink channel to send the message
- *
- * @param Vcc board voltage (mV)
- * @param I2Cerr I2C error count
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_HWSTATUS_LEN];
-	_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;
-	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
-
-// MESSAGE HWSTATUS UNPACKING
-
-
-/**
- * @brief Get field Vcc from hwstatus message
- *
- * @return board voltage (mV)
- */
-static inline uint16_t mavlink_msg_hwstatus_get_Vcc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field I2Cerr from hwstatus message
- *
- * @return I2C error count
- */
-static inline uint8_t mavlink_msg_hwstatus_get_I2Cerr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a hwstatus message into a struct
- *
- * @param msg The message to decode
- * @param hwstatus C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hwstatus_decode(const mavlink_message_t* msg, mavlink_hwstatus_t* hwstatus)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hwstatus->Vcc = mavlink_msg_hwstatus_get_Vcc(msg);
-	hwstatus->I2Cerr = mavlink_msg_hwstatus_get_I2Cerr(msg);
-#else
-	memcpy(hwstatus, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-}
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
deleted file mode 100644
index 34743fd02..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h
+++ /dev/null
@@ -1,353 +0,0 @@
-// MESSAGE LIMITS_STATUS PACKING
-
-#define MAVLINK_MSG_ID_LIMITS_STATUS 167
-
-typedef struct __mavlink_limits_status_t
-{
- uint32_t last_trigger; ///< time of last breach in milliseconds since boot
- uint32_t last_action; ///< time of last recovery action in milliseconds since boot
- uint32_t last_recovery; ///< time of last successful recovery in milliseconds since boot
- uint32_t last_clear; ///< time of last all-clear in milliseconds since boot
- uint16_t breach_count; ///< number of fence breaches
- uint8_t limits_state; ///< state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- uint8_t mods_enabled; ///< AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- uint8_t mods_required; ///< AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- uint8_t mods_triggered; ///< AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
-} mavlink_limits_status_t;
-
-#define MAVLINK_MSG_ID_LIMITS_STATUS_LEN 22
-#define MAVLINK_MSG_ID_167_LEN 22
-
-#define MAVLINK_MSG_ID_LIMITS_STATUS_CRC 144
-#define MAVLINK_MSG_ID_167_CRC 144
-
-
-
-#define MAVLINK_MESSAGE_INFO_LIMITS_STATUS { \
-	"LIMITS_STATUS", \
-	9, \
-	{  { "last_trigger", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_limits_status_t, last_trigger) }, \
-         { "last_action", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_limits_status_t, last_action) }, \
-         { "last_recovery", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_limits_status_t, last_recovery) }, \
-         { "last_clear", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_limits_status_t, last_clear) }, \
-         { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_limits_status_t, breach_count) }, \
-         { "limits_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_limits_status_t, limits_state) }, \
-         { "mods_enabled", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_limits_status_t, mods_enabled) }, \
-         { "mods_required", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_limits_status_t, mods_required) }, \
-         { "mods_triggered", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_limits_status_t, mods_triggered) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a limits_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 limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- * @param last_trigger time of last breach in milliseconds since boot
- * @param last_action time of last recovery action in milliseconds since boot
- * @param last_recovery time of last successful recovery in milliseconds since boot
- * @param last_clear time of last all-clear in milliseconds since boot
- * @param breach_count number of fence breaches
- * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_LIMITS_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#else
-	mavlink_limits_status_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a limits_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 limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- * @param last_trigger time of last breach in milliseconds since boot
- * @param last_action time of last recovery action in milliseconds since boot
- * @param last_recovery time of last successful recovery in milliseconds since boot
- * @param last_clear time of last all-clear in milliseconds since boot
- * @param breach_count number of fence breaches
- * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_LIMITS_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#else
-	mavlink_limits_status_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a limits_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 limits_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status)
-{
-	return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered);
-}
-
-/**
- * @brief Encode a limits_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 limits_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_limits_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status)
-{
-	return mavlink_msg_limits_status_pack_chan(system_id, component_id, chan, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered);
-}
-
-/**
- * @brief Send a limits_status message
- * @param chan MAVLink channel to send the message
- *
- * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- * @param last_trigger time of last breach in milliseconds since boot
- * @param last_action time of last recovery action in milliseconds since boot
- * @param last_recovery time of last successful recovery in milliseconds since boot
- * @param last_clear time of last all-clear in milliseconds since boot
- * @param breach_count number of fence breaches
- * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_limits_status_send(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[MAVLINK_MSG_ID_LIMITS_STATUS_LEN];
-	_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;
-	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
-
-// MESSAGE LIMITS_STATUS UNPACKING
-
-
-/**
- * @brief Get field limits_state from limits_status message
- *
- * @return state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- */
-static inline uint8_t mavlink_msg_limits_status_get_limits_state(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  18);
-}
-
-/**
- * @brief Get field last_trigger from limits_status message
- *
- * @return time of last breach in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_limits_status_get_last_trigger(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field last_action from limits_status message
- *
- * @return time of last recovery action in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_limits_status_get_last_action(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  4);
-}
-
-/**
- * @brief Get field last_recovery from limits_status message
- *
- * @return time of last successful recovery in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_limits_status_get_last_recovery(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  8);
-}
-
-/**
- * @brief Get field last_clear from limits_status message
- *
- * @return time of last all-clear in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_limits_status_get_last_clear(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  12);
-}
-
-/**
- * @brief Get field breach_count from limits_status message
- *
- * @return number of fence breaches
- */
-static inline uint16_t mavlink_msg_limits_status_get_breach_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field mods_enabled from limits_status message
- *
- * @return AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- */
-static inline uint8_t mavlink_msg_limits_status_get_mods_enabled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  19);
-}
-
-/**
- * @brief Get field mods_required from limits_status message
- *
- * @return AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- */
-static inline uint8_t mavlink_msg_limits_status_get_mods_required(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  20);
-}
-
-/**
- * @brief Get field mods_triggered from limits_status message
- *
- * @return AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
- */
-static inline uint8_t mavlink_msg_limits_status_get_mods_triggered(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  21);
-}
-
-/**
- * @brief Decode a limits_status message into a struct
- *
- * @param msg The message to decode
- * @param limits_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_limits_status_decode(const mavlink_message_t* msg, mavlink_limits_status_t* limits_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	limits_status->last_trigger = mavlink_msg_limits_status_get_last_trigger(msg);
-	limits_status->last_action = mavlink_msg_limits_status_get_last_action(msg);
-	limits_status->last_recovery = mavlink_msg_limits_status_get_last_recovery(msg);
-	limits_status->last_clear = mavlink_msg_limits_status_get_last_clear(msg);
-	limits_status->breach_count = mavlink_msg_limits_status_get_breach_count(msg);
-	limits_status->limits_state = mavlink_msg_limits_status_get_limits_state(msg);
-	limits_status->mods_enabled = mavlink_msg_limits_status_get_mods_enabled(msg);
-	limits_status->mods_required = mavlink_msg_limits_status_get_mods_required(msg);
-	limits_status->mods_triggered = mavlink_msg_limits_status_get_mods_triggered(msg);
-#else
-	memcpy(limits_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h
deleted file mode 100644
index 55f772bbc..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE MEMINFO PACKING
-
-#define MAVLINK_MSG_ID_MEMINFO 152
-
-typedef struct __mavlink_meminfo_t
-{
- uint16_t brkval; ///< heap top
- uint16_t freemem; ///< free memory
-} mavlink_meminfo_t;
-
-#define MAVLINK_MSG_ID_MEMINFO_LEN 4
-#define MAVLINK_MSG_ID_152_LEN 4
-
-#define MAVLINK_MSG_ID_MEMINFO_CRC 208
-#define MAVLINK_MSG_ID_152_CRC 208
-
-
-
-#define MAVLINK_MESSAGE_INFO_MEMINFO { \
-	"MEMINFO", \
-	2, \
-	{  { "brkval", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_meminfo_t, brkval) }, \
-         { "freemem", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_meminfo_t, freemem) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a meminfo 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 brkval heap top
- * @param freemem free memory
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t brkval, uint16_t freemem)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MEMINFO_LEN];
-	_mav_put_uint16_t(buf, 0, brkval);
-	_mav_put_uint16_t(buf, 2, freemem);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN);
-#else
-	mavlink_meminfo_t packet;
-	packet.brkval = brkval;
-	packet.freemem = freemem;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MEMINFO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-}
-
-/**
- * @brief Pack a meminfo 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 brkval heap top
- * @param freemem free memory
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t brkval,uint16_t freemem)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MEMINFO_LEN];
-	_mav_put_uint16_t(buf, 0, brkval);
-	_mav_put_uint16_t(buf, 2, freemem);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN);
-#else
-	mavlink_meminfo_t packet;
-	packet.brkval = brkval;
-	packet.freemem = freemem;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MEMINFO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-}
-
-/**
- * @brief Encode a meminfo 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 meminfo C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo)
-{
-	return mavlink_msg_meminfo_pack(system_id, component_id, msg, meminfo->brkval, meminfo->freemem);
-}
-
-/**
- * @brief Encode a meminfo 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 meminfo C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_meminfo_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo)
-{
-	return mavlink_msg_meminfo_pack_chan(system_id, component_id, chan, msg, meminfo->brkval, meminfo->freemem);
-}
-
-/**
- * @brief Send a meminfo message
- * @param chan MAVLink channel to send the message
- *
- * @param brkval heap top
- * @param freemem free memory
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_meminfo_send(mavlink_channel_t chan, uint16_t brkval, uint16_t freemem)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MEMINFO_LEN];
-	_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;
-	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
-
-// MESSAGE MEMINFO UNPACKING
-
-
-/**
- * @brief Get field brkval from meminfo message
- *
- * @return heap top
- */
-static inline uint16_t mavlink_msg_meminfo_get_brkval(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field freemem from meminfo message
- *
- * @return free memory
- */
-static inline uint16_t mavlink_msg_meminfo_get_freemem(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a meminfo message into a struct
- *
- * @param msg The message to decode
- * @param meminfo C-struct to decode the message contents into
- */
-static inline void mavlink_msg_meminfo_decode(const mavlink_message_t* msg, mavlink_meminfo_t* meminfo)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	meminfo->brkval = mavlink_msg_meminfo_get_brkval(msg);
-	meminfo->freemem = mavlink_msg_meminfo_get_freemem(msg);
-#else
-	memcpy(meminfo, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-}
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
deleted file mode 100644
index de717dfa4..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE MOUNT_CONFIGURE PACKING
-
-#define MAVLINK_MSG_ID_MOUNT_CONFIGURE 156
-
-typedef struct __mavlink_mount_configure_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t mount_mode; ///< mount operating mode (see MAV_MOUNT_MODE enum)
- uint8_t stab_roll; ///< (1 = yes, 0 = no)
- uint8_t stab_pitch; ///< (1 = yes, 0 = no)
- uint8_t stab_yaw; ///< (1 = yes, 0 = no)
-} mavlink_mount_configure_t;
-
-#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN 6
-#define MAVLINK_MSG_ID_156_LEN 6
-
-#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC 19
-#define MAVLINK_MSG_ID_156_CRC 19
-
-
-
-#define MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE { \
-	"MOUNT_CONFIGURE", \
-	6, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mount_configure_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mount_configure_t, target_component) }, \
-         { "mount_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mount_configure_t, mount_mode) }, \
-         { "stab_roll", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mount_configure_t, stab_roll) }, \
-         { "stab_pitch", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mount_configure_t, stab_pitch) }, \
-         { "stab_yaw", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mount_configure_t, stab_yaw) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mount_configure 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 target_system System ID
- * @param target_component Component ID
- * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum)
- * @param stab_roll (1 = yes, 0 = no)
- * @param stab_pitch (1 = yes, 0 = no)
- * @param stab_yaw (1 = yes, 0 = no)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#else
-	mavlink_mount_configure_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mount_configure 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 target_system System ID
- * @param target_component Component ID
- * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum)
- * @param stab_roll (1 = yes, 0 = no)
- * @param stab_pitch (1 = yes, 0 = no)
- * @param stab_yaw (1 = yes, 0 = no)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#else
-	mavlink_mount_configure_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mount_configure 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 mount_configure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure)
-{
-	return mavlink_msg_mount_configure_pack(system_id, component_id, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw);
-}
-
-/**
- * @brief Encode a mount_configure 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 mount_configure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure)
-{
-	return mavlink_msg_mount_configure_pack_chan(system_id, component_id, chan, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw);
-}
-
-/**
- * @brief Send a mount_configure message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum)
- * @param stab_roll (1 = yes, 0 = no)
- * @param stab_pitch (1 = yes, 0 = no)
- * @param stab_yaw (1 = yes, 0 = no)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mount_configure_send(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[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN];
-	_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;
-	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
-
-// MESSAGE MOUNT_CONFIGURE UNPACKING
-
-
-/**
- * @brief Get field target_system from mount_configure message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mount_configure_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from mount_configure message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mount_configure_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field mount_mode from mount_configure message
- *
- * @return mount operating mode (see MAV_MOUNT_MODE enum)
- */
-static inline uint8_t mavlink_msg_mount_configure_get_mount_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field stab_roll from mount_configure message
- *
- * @return (1 = yes, 0 = no)
- */
-static inline uint8_t mavlink_msg_mount_configure_get_stab_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field stab_pitch from mount_configure message
- *
- * @return (1 = yes, 0 = no)
- */
-static inline uint8_t mavlink_msg_mount_configure_get_stab_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field stab_yaw from mount_configure message
- *
- * @return (1 = yes, 0 = no)
- */
-static inline uint8_t mavlink_msg_mount_configure_get_stab_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Decode a mount_configure message into a struct
- *
- * @param msg The message to decode
- * @param mount_configure C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mount_configure_decode(const mavlink_message_t* msg, mavlink_mount_configure_t* mount_configure)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mount_configure->target_system = mavlink_msg_mount_configure_get_target_system(msg);
-	mount_configure->target_component = mavlink_msg_mount_configure_get_target_component(msg);
-	mount_configure->mount_mode = mavlink_msg_mount_configure_get_mount_mode(msg);
-	mount_configure->stab_roll = mavlink_msg_mount_configure_get_stab_roll(msg);
-	mount_configure->stab_pitch = mavlink_msg_mount_configure_get_stab_pitch(msg);
-	mount_configure->stab_yaw = mavlink_msg_mount_configure_get_stab_yaw(msg);
-#else
-	memcpy(mount_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-}
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
deleted file mode 100644
index 44416353e..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE MOUNT_CONTROL PACKING
-
-#define MAVLINK_MSG_ID_MOUNT_CONTROL 157
-
-typedef struct __mavlink_mount_control_t
-{
- int32_t input_a; ///< pitch(deg*100) or lat, depending on mount mode
- int32_t input_b; ///< roll(deg*100) or lon depending on mount mode
- int32_t input_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t save_position; ///< if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
-} mavlink_mount_control_t;
-
-#define MAVLINK_MSG_ID_MOUNT_CONTROL_LEN 15
-#define MAVLINK_MSG_ID_157_LEN 15
-
-#define MAVLINK_MSG_ID_MOUNT_CONTROL_CRC 21
-#define MAVLINK_MSG_ID_157_CRC 21
-
-
-
-#define MAVLINK_MESSAGE_INFO_MOUNT_CONTROL { \
-	"MOUNT_CONTROL", \
-	6, \
-	{  { "input_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_control_t, input_a) }, \
-         { "input_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_control_t, input_b) }, \
-         { "input_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_control_t, input_c) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_control_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_control_t, target_component) }, \
-         { "save_position", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_mount_control_t, save_position) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mount_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 target_system System ID
- * @param target_component Component ID
- * @param input_a pitch(deg*100) or lat, depending on mount mode
- * @param input_b roll(deg*100) or lon depending on mount mode
- * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#else
-	mavlink_mount_control_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mount_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 target_system System ID
- * @param target_component Component ID
- * @param input_a pitch(deg*100) or lat, depending on mount mode
- * @param input_b roll(deg*100) or lon depending on mount mode
- * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#else
-	mavlink_mount_control_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mount_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 mount_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control)
-{
-	return mavlink_msg_mount_control_pack(system_id, component_id, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position);
-}
-
-/**
- * @brief Encode a mount_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 mount_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control)
-{
-	return mavlink_msg_mount_control_pack_chan(system_id, component_id, chan, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position);
-}
-
-/**
- * @brief Send a mount_control message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param input_a pitch(deg*100) or lat, depending on mount mode
- * @param input_b roll(deg*100) or lon depending on mount mode
- * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mount_control_send(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[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN];
-	_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;
-	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
-
-// MESSAGE MOUNT_CONTROL UNPACKING
-
-
-/**
- * @brief Get field target_system from mount_control message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mount_control_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field target_component from mount_control message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mount_control_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  13);
-}
-
-/**
- * @brief Get field input_a from mount_control message
- *
- * @return pitch(deg*100) or lat, depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_control_get_input_a(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field input_b from mount_control message
- *
- * @return roll(deg*100) or lon depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_control_get_input_b(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field input_c from mount_control message
- *
- * @return yaw(deg*100) or alt (in cm) depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_control_get_input_c(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field save_position from mount_control message
- *
- * @return if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
- */
-static inline uint8_t mavlink_msg_mount_control_get_save_position(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Decode a mount_control message into a struct
- *
- * @param msg The message to decode
- * @param mount_control C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mount_control_decode(const mavlink_message_t* msg, mavlink_mount_control_t* mount_control)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mount_control->input_a = mavlink_msg_mount_control_get_input_a(msg);
-	mount_control->input_b = mavlink_msg_mount_control_get_input_b(msg);
-	mount_control->input_c = mavlink_msg_mount_control_get_input_c(msg);
-	mount_control->target_system = mavlink_msg_mount_control_get_target_system(msg);
-	mount_control->target_component = mavlink_msg_mount_control_get_target_component(msg);
-	mount_control->save_position = mavlink_msg_mount_control_get_save_position(msg);
-#else
-	memcpy(mount_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-}
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
deleted file mode 100644
index 4905905dc..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE MOUNT_STATUS PACKING
-
-#define MAVLINK_MSG_ID_MOUNT_STATUS 158
-
-typedef struct __mavlink_mount_status_t
-{
- int32_t pointing_a; ///< pitch(deg*100) or lat, depending on mount mode
- int32_t pointing_b; ///< roll(deg*100) or lon depending on mount mode
- int32_t pointing_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mount_status_t;
-
-#define MAVLINK_MSG_ID_MOUNT_STATUS_LEN 14
-#define MAVLINK_MSG_ID_158_LEN 14
-
-#define MAVLINK_MSG_ID_MOUNT_STATUS_CRC 134
-#define MAVLINK_MSG_ID_158_CRC 134
-
-
-
-#define MAVLINK_MESSAGE_INFO_MOUNT_STATUS { \
-	"MOUNT_STATUS", \
-	5, \
-	{  { "pointing_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_status_t, pointing_a) }, \
-         { "pointing_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_status_t, pointing_b) }, \
-         { "pointing_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_status_t, pointing_c) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_status_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_status_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mount_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 target_system System ID
- * @param target_component Component ID
- * @param pointing_a pitch(deg*100) or lat, depending on mount mode
- * @param pointing_b roll(deg*100) or lon depending on mount mode
- * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MOUNT_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#else
-	mavlink_mount_status_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mount_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 target_system System ID
- * @param target_component Component ID
- * @param pointing_a pitch(deg*100) or lat, depending on mount mode
- * @param pointing_b roll(deg*100) or lon depending on mount mode
- * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MOUNT_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#else
-	mavlink_mount_status_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mount_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 mount_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status)
-{
-	return mavlink_msg_mount_status_pack(system_id, component_id, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c);
-}
-
-/**
- * @brief Encode a mount_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 mount_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status)
-{
-	return mavlink_msg_mount_status_pack_chan(system_id, component_id, chan, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c);
-}
-
-/**
- * @brief Send a mount_status message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param pointing_a pitch(deg*100) or lat, depending on mount mode
- * @param pointing_b roll(deg*100) or lon depending on mount mode
- * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mount_status_send(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[MAVLINK_MSG_ID_MOUNT_STATUS_LEN];
-	_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;
-	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
-
-// MESSAGE MOUNT_STATUS UNPACKING
-
-
-/**
- * @brief Get field target_system from mount_status message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mount_status_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field target_component from mount_status message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mount_status_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  13);
-}
-
-/**
- * @brief Get field pointing_a from mount_status message
- *
- * @return pitch(deg*100) or lat, depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_status_get_pointing_a(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field pointing_b from mount_status message
- *
- * @return roll(deg*100) or lon depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_status_get_pointing_b(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field pointing_c from mount_status message
- *
- * @return yaw(deg*100) or alt (in cm) depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_status_get_pointing_c(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Decode a mount_status message into a struct
- *
- * @param msg The message to decode
- * @param mount_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mount_status_decode(const mavlink_message_t* msg, mavlink_mount_status_t* mount_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mount_status->pointing_a = mavlink_msg_mount_status_get_pointing_a(msg);
-	mount_status->pointing_b = mavlink_msg_mount_status_get_pointing_b(msg);
-	mount_status->pointing_c = mavlink_msg_mount_status_get_pointing_c(msg);
-	mount_status->target_system = mavlink_msg_mount_status_get_target_system(msg);
-	mount_status->target_component = mavlink_msg_mount_status_get_target_component(msg);
-#else
-	memcpy(mount_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h
deleted file mode 100644
index 8e9740e82..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE RADIO PACKING
-
-#define MAVLINK_MSG_ID_RADIO 166
-
-typedef struct __mavlink_radio_t
-{
- uint16_t rxerrors; ///< receive errors
- uint16_t fixed; ///< count of error corrected packets
- uint8_t rssi; ///< local signal strength
- uint8_t remrssi; ///< remote signal strength
- uint8_t txbuf; ///< how full the tx buffer is as a percentage
- uint8_t noise; ///< background noise level
- uint8_t remnoise; ///< remote background noise level
-} mavlink_radio_t;
-
-#define MAVLINK_MSG_ID_RADIO_LEN 9
-#define MAVLINK_MSG_ID_166_LEN 9
-
-#define MAVLINK_MSG_ID_RADIO_CRC 21
-#define MAVLINK_MSG_ID_166_CRC 21
-
-
-
-#define MAVLINK_MESSAGE_INFO_RADIO { \
-	"RADIO", \
-	7, \
-	{  { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_t, rxerrors) }, \
-         { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_t, fixed) }, \
-         { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_t, rssi) }, \
-         { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_t, remrssi) }, \
-         { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_t, txbuf) }, \
-         { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_t, noise) }, \
-         { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_t, remnoise) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a radio 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 rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RADIO_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN);
-#else
-	mavlink_radio_t packet;
-	packet.rxerrors = rxerrors;
-	packet.fixed = fixed;
-	packet.rssi = rssi;
-	packet.remrssi = remrssi;
-	packet.txbuf = txbuf;
-	packet.noise = noise;
-	packet.remnoise = remnoise;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RADIO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-}
-
-/**
- * @brief Pack a radio 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 rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RADIO_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN);
-#else
-	mavlink_radio_t packet;
-	packet.rxerrors = rxerrors;
-	packet.fixed = fixed;
-	packet.rssi = rssi;
-	packet.remrssi = remrssi;
-	packet.txbuf = txbuf;
-	packet.noise = noise;
-	packet.remnoise = remnoise;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RADIO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-}
-
-/**
- * @brief Encode a radio 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 radio C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio)
-{
-	return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed);
-}
-
-/**
- * @brief Encode a radio 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 radio C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_radio_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_t* radio)
-{
-	return mavlink_msg_radio_pack_chan(system_id, component_id, chan, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed);
-}
-
-/**
- * @brief Send a radio message
- * @param chan MAVLink channel to send the message
- *
- * @param rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_radio_send(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[MAVLINK_MSG_ID_RADIO_LEN];
-	_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;
-	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
-
-// MESSAGE RADIO UNPACKING
-
-
-/**
- * @brief Get field rssi from radio message
- *
- * @return local signal strength
- */
-static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field remrssi from radio message
- *
- * @return remote signal strength
- */
-static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field txbuf from radio message
- *
- * @return how full the tx buffer is as a percentage
- */
-static inline uint8_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field noise from radio message
- *
- * @return background noise level
- */
-static inline uint8_t mavlink_msg_radio_get_noise(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field remnoise from radio message
- *
- * @return remote background noise level
- */
-static inline uint8_t mavlink_msg_radio_get_remnoise(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field rxerrors from radio message
- *
- * @return receive errors
- */
-static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field fixed from radio message
- *
- * @return count of error corrected packets
- */
-static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a radio message into a struct
- *
- * @param msg The message to decode
- * @param radio C-struct to decode the message contents into
- */
-static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlink_radio_t* radio)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg);
-	radio->fixed = mavlink_msg_radio_get_fixed(msg);
-	radio->rssi = mavlink_msg_radio_get_rssi(msg);
-	radio->remrssi = mavlink_msg_radio_get_remrssi(msg);
-	radio->txbuf = mavlink_msg_radio_get_txbuf(msg);
-	radio->noise = mavlink_msg_radio_get_noise(msg);
-	radio->remnoise = mavlink_msg_radio_get_remnoise(msg);
-#else
-	memcpy(radio, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-}
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
deleted file mode 100644
index f057e3c33..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE RALLY_FETCH_POINT PACKING
-
-#define MAVLINK_MSG_ID_RALLY_FETCH_POINT 176
-
-typedef struct __mavlink_rally_fetch_point_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t idx; ///< point index (first point is 0)
-} mavlink_rally_fetch_point_t;
-
-#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN 3
-#define MAVLINK_MSG_ID_176_LEN 3
-
-#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC 234
-#define MAVLINK_MSG_ID_176_CRC 234
-
-
-
-#define MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT { \
-	"RALLY_FETCH_POINT", \
-	3, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rally_fetch_point_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rally_fetch_point_t, target_component) }, \
-         { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_rally_fetch_point_t, idx) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a rally_fetch_point 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 target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 0)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rally_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t idx)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, idx);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
-#else
-	mavlink_rally_fetch_point_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a rally_fetch_point 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 target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 0)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rally_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t idx)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, idx);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
-#else
-	mavlink_rally_fetch_point_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a rally_fetch_point 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 rally_fetch_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rally_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point)
-{
-	return mavlink_msg_rally_fetch_point_pack(system_id, component_id, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx);
-}
-
-/**
- * @brief Encode a rally_fetch_point 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 rally_fetch_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rally_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point)
-{
-	return mavlink_msg_rally_fetch_point_pack_chan(system_id, component_id, chan, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx);
-}
-
-/**
- * @brief Send a rally_fetch_point message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 0)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_rally_fetch_point_send(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[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN];
-	_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;
-	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
-
-// MESSAGE RALLY_FETCH_POINT UNPACKING
-
-
-/**
- * @brief Get field target_system from rally_fetch_point message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_rally_fetch_point_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from rally_fetch_point message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_rally_fetch_point_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field idx from rally_fetch_point message
- *
- * @return point index (first point is 0)
- */
-static inline uint8_t mavlink_msg_rally_fetch_point_get_idx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a rally_fetch_point message into a struct
- *
- * @param msg The message to decode
- * @param rally_fetch_point C-struct to decode the message contents into
- */
-static inline void mavlink_msg_rally_fetch_point_decode(const mavlink_message_t* msg, mavlink_rally_fetch_point_t* rally_fetch_point)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	rally_fetch_point->target_system = mavlink_msg_rally_fetch_point_get_target_system(msg);
-	rally_fetch_point->target_component = mavlink_msg_rally_fetch_point_get_target_component(msg);
-	rally_fetch_point->idx = mavlink_msg_rally_fetch_point_get_idx(msg);
-#else
-	memcpy(rally_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
-#endif
-}
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
deleted file mode 100644
index 2c21db4c0..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE RALLY_POINT PACKING
-
-#define MAVLINK_MSG_ID_RALLY_POINT 175
-
-typedef struct __mavlink_rally_point_t
-{
- int32_t lat; ///< Latitude of point in degrees * 1E7
- int32_t lng; ///< Longitude of point in degrees * 1E7
- int16_t alt; ///< Transit / loiter altitude in meters relative to home
- int16_t break_alt; ///< Break altitude in meters relative to home
- uint16_t land_dir; ///< Heading to aim for when landing. In centi-degrees.
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t idx; ///< point index (first point is 0)
- uint8_t count; ///< total number of points (for sanity checking)
- uint8_t flags; ///< See RALLY_FLAGS enum for definition of the bitmask.
-} mavlink_rally_point_t;
-
-#define MAVLINK_MSG_ID_RALLY_POINT_LEN 19
-#define MAVLINK_MSG_ID_175_LEN 19
-
-#define MAVLINK_MSG_ID_RALLY_POINT_CRC 138
-#define MAVLINK_MSG_ID_175_CRC 138
-
-
-
-#define MAVLINK_MESSAGE_INFO_RALLY_POINT { \
-	"RALLY_POINT", \
-	10, \
-	{  { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_rally_point_t, lat) }, \
-         { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_rally_point_t, lng) }, \
-         { "alt", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rally_point_t, alt) }, \
-         { "break_alt", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rally_point_t, break_alt) }, \
-         { "land_dir", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rally_point_t, land_dir) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_rally_point_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_rally_point_t, target_component) }, \
-         { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rally_point_t, idx) }, \
-         { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rally_point_t, count) }, \
-         { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_rally_point_t, flags) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a rally_point 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 target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 0)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point in degrees * 1E7
- * @param lng Longitude of point in degrees * 1E7
- * @param alt Transit / loiter altitude in meters relative to home
- * @param break_alt Break altitude in meters relative to home
- * @param land_dir Heading to aim for when landing. In centi-degrees.
- * @param flags See RALLY_FLAGS enum for definition of the bitmask.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rally_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RALLY_POINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN);
-#else
-	mavlink_rally_point_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RALLY_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a rally_point 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 target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 0)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point in degrees * 1E7
- * @param lng Longitude of point in degrees * 1E7
- * @param alt Transit / loiter altitude in meters relative to home
- * @param break_alt Break altitude in meters relative to home
- * @param land_dir Heading to aim for when landing. In centi-degrees.
- * @param flags See RALLY_FLAGS enum for definition of the bitmask.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rally_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RALLY_POINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN);
-#else
-	mavlink_rally_point_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RALLY_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a rally_point 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 rally_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rally_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point)
-{
-	return mavlink_msg_rally_point_pack(system_id, component_id, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags);
-}
-
-/**
- * @brief Encode a rally_point 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 rally_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rally_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point)
-{
-	return mavlink_msg_rally_point_pack_chan(system_id, component_id, chan, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags);
-}
-
-/**
- * @brief Send a rally_point message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 0)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point in degrees * 1E7
- * @param lng Longitude of point in degrees * 1E7
- * @param alt Transit / loiter altitude in meters relative to home
- * @param break_alt Break altitude in meters relative to home
- * @param land_dir Heading to aim for when landing. In centi-degrees.
- * @param flags See RALLY_FLAGS enum for definition of the bitmask.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_rally_point_send(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[MAVLINK_MSG_ID_RALLY_POINT_LEN];
-	_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;
-	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
-
-// MESSAGE RALLY_POINT UNPACKING
-
-
-/**
- * @brief Get field target_system from rally_point message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_rally_point_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Get field target_component from rally_point message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_rally_point_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  15);
-}
-
-/**
- * @brief Get field idx from rally_point message
- *
- * @return point index (first point is 0)
- */
-static inline uint8_t mavlink_msg_rally_point_get_idx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field count from rally_point message
- *
- * @return total number of points (for sanity checking)
- */
-static inline uint8_t mavlink_msg_rally_point_get_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  17);
-}
-
-/**
- * @brief Get field lat from rally_point message
- *
- * @return Latitude of point in degrees * 1E7
- */
-static inline int32_t mavlink_msg_rally_point_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field lng from rally_point message
- *
- * @return Longitude of point in degrees * 1E7
- */
-static inline int32_t mavlink_msg_rally_point_get_lng(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field alt from rally_point message
- *
- * @return Transit / loiter altitude in meters relative to home
- */
-static inline int16_t mavlink_msg_rally_point_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field break_alt from rally_point message
- *
- * @return Break altitude in meters relative to home
- */
-static inline int16_t mavlink_msg_rally_point_get_break_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  10);
-}
-
-/**
- * @brief Get field land_dir from rally_point message
- *
- * @return Heading to aim for when landing. In centi-degrees.
- */
-static inline uint16_t mavlink_msg_rally_point_get_land_dir(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field flags from rally_point message
- *
- * @return See RALLY_FLAGS enum for definition of the bitmask.
- */
-static inline uint8_t mavlink_msg_rally_point_get_flags(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  18);
-}
-
-/**
- * @brief Decode a rally_point message into a struct
- *
- * @param msg The message to decode
- * @param rally_point C-struct to decode the message contents into
- */
-static inline void mavlink_msg_rally_point_decode(const mavlink_message_t* msg, mavlink_rally_point_t* rally_point)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	rally_point->lat = mavlink_msg_rally_point_get_lat(msg);
-	rally_point->lng = mavlink_msg_rally_point_get_lng(msg);
-	rally_point->alt = mavlink_msg_rally_point_get_alt(msg);
-	rally_point->break_alt = mavlink_msg_rally_point_get_break_alt(msg);
-	rally_point->land_dir = mavlink_msg_rally_point_get_land_dir(msg);
-	rally_point->target_system = mavlink_msg_rally_point_get_target_system(msg);
-	rally_point->target_component = mavlink_msg_rally_point_get_target_component(msg);
-	rally_point->idx = mavlink_msg_rally_point_get_idx(msg);
-	rally_point->count = mavlink_msg_rally_point_get_count(msg);
-	rally_point->flags = mavlink_msg_rally_point_get_flags(msg);
-#else
-	memcpy(rally_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_POINT_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h
deleted file mode 100644
index c476447a8..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE RANGEFINDER PACKING
-
-#define MAVLINK_MSG_ID_RANGEFINDER 173
-
-typedef struct __mavlink_rangefinder_t
-{
- float distance; ///< distance in meters
- float voltage; ///< raw voltage if available, zero otherwise
-} mavlink_rangefinder_t;
-
-#define MAVLINK_MSG_ID_RANGEFINDER_LEN 8
-#define MAVLINK_MSG_ID_173_LEN 8
-
-#define MAVLINK_MSG_ID_RANGEFINDER_CRC 83
-#define MAVLINK_MSG_ID_173_CRC 83
-
-
-
-#define MAVLINK_MESSAGE_INFO_RANGEFINDER { \
-	"RANGEFINDER", \
-	2, \
-	{  { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rangefinder_t, distance) }, \
-         { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rangefinder_t, voltage) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a rangefinder 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 distance distance in meters
- * @param voltage raw voltage if available, zero otherwise
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float distance, float voltage)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN];
-	_mav_put_float(buf, 0, distance);
-	_mav_put_float(buf, 4, voltage);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#else
-	mavlink_rangefinder_t packet;
-	packet.distance = distance;
-	packet.voltage = voltage;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RANGEFINDER;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-}
-
-/**
- * @brief Pack a rangefinder 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 distance distance in meters
- * @param voltage raw voltage if available, zero otherwise
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float distance,float voltage)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN];
-	_mav_put_float(buf, 0, distance);
-	_mav_put_float(buf, 4, voltage);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#else
-	mavlink_rangefinder_t packet;
-	packet.distance = distance;
-	packet.voltage = voltage;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RANGEFINDER;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-}
-
-/**
- * @brief Encode a rangefinder 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 rangefinder C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder)
-{
-	return mavlink_msg_rangefinder_pack(system_id, component_id, msg, rangefinder->distance, rangefinder->voltage);
-}
-
-/**
- * @brief Encode a rangefinder 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 rangefinder C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rangefinder_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder)
-{
-	return mavlink_msg_rangefinder_pack_chan(system_id, component_id, chan, msg, rangefinder->distance, rangefinder->voltage);
-}
-
-/**
- * @brief Send a rangefinder message
- * @param chan MAVLink channel to send the message
- *
- * @param distance distance in meters
- * @param voltage raw voltage if available, zero otherwise
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float distance, float voltage)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN];
-	_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;
-	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
-
-// MESSAGE RANGEFINDER UNPACKING
-
-
-/**
- * @brief Get field distance from rangefinder message
- *
- * @return distance in meters
- */
-static inline float mavlink_msg_rangefinder_get_distance(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field voltage from rangefinder message
- *
- * @return raw voltage if available, zero otherwise
- */
-static inline float mavlink_msg_rangefinder_get_voltage(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Decode a rangefinder message into a struct
- *
- * @param msg The message to decode
- * @param rangefinder C-struct to decode the message contents into
- */
-static inline void mavlink_msg_rangefinder_decode(const mavlink_message_t* msg, mavlink_rangefinder_t* rangefinder)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	rangefinder->distance = mavlink_msg_rangefinder_get_distance(msg);
-	rangefinder->voltage = mavlink_msg_rangefinder_get_voltage(msg);
-#else
-	memcpy(rangefinder, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-}
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
deleted file mode 100644
index 31b7d989d..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h
+++ /dev/null
@@ -1,419 +0,0 @@
-// MESSAGE SENSOR_OFFSETS PACKING
-
-#define MAVLINK_MSG_ID_SENSOR_OFFSETS 150
-
-typedef struct __mavlink_sensor_offsets_t
-{
- float mag_declination; ///< magnetic declination (radians)
- int32_t raw_press; ///< raw pressure from barometer
- int32_t raw_temp; ///< raw temperature from barometer
- float gyro_cal_x; ///< gyro X calibration
- float gyro_cal_y; ///< gyro Y calibration
- float gyro_cal_z; ///< gyro Z calibration
- float accel_cal_x; ///< accel X calibration
- float accel_cal_y; ///< accel Y calibration
- float accel_cal_z; ///< accel Z calibration
- int16_t mag_ofs_x; ///< magnetometer X offset
- int16_t mag_ofs_y; ///< magnetometer Y offset
- int16_t mag_ofs_z; ///< magnetometer Z offset
-} mavlink_sensor_offsets_t;
-
-#define MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN 42
-#define MAVLINK_MSG_ID_150_LEN 42
-
-#define MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC 134
-#define MAVLINK_MSG_ID_150_CRC 134
-
-
-
-#define MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS { \
-	"SENSOR_OFFSETS", \
-	12, \
-	{  { "mag_declination", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_offsets_t, mag_declination) }, \
-         { "raw_press", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_sensor_offsets_t, raw_press) }, \
-         { "raw_temp", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_sensor_offsets_t, raw_temp) }, \
-         { "gyro_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_offsets_t, gyro_cal_x) }, \
-         { "gyro_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_offsets_t, gyro_cal_y) }, \
-         { "gyro_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_offsets_t, gyro_cal_z) }, \
-         { "accel_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sensor_offsets_t, accel_cal_x) }, \
-         { "accel_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sensor_offsets_t, accel_cal_y) }, \
-         { "accel_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sensor_offsets_t, accel_cal_z) }, \
-         { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_sensor_offsets_t, mag_ofs_x) }, \
-         { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_sensor_offsets_t, mag_ofs_y) }, \
-         { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_sensor_offsets_t, mag_ofs_z) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a sensor_offsets 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 mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @param mag_declination magnetic declination (radians)
- * @param raw_press raw pressure from barometer
- * @param raw_temp raw temperature from barometer
- * @param gyro_cal_x gyro X calibration
- * @param gyro_cal_y gyro Y calibration
- * @param gyro_cal_z gyro Z calibration
- * @param accel_cal_x accel X calibration
- * @param accel_cal_y accel Y calibration
- * @param accel_cal_z accel Z calibration
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#else
-	mavlink_sensor_offsets_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a sensor_offsets 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 mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @param mag_declination magnetic declination (radians)
- * @param raw_press raw pressure from barometer
- * @param raw_temp raw temperature from barometer
- * @param gyro_cal_x gyro X calibration
- * @param gyro_cal_y gyro Y calibration
- * @param gyro_cal_z gyro Z calibration
- * @param accel_cal_x accel X calibration
- * @param accel_cal_y accel Y calibration
- * @param accel_cal_z accel Z calibration
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#else
-	mavlink_sensor_offsets_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a sensor_offsets 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 sensor_offsets C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets)
-{
-	return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z);
-}
-
-/**
- * @brief Encode a sensor_offsets 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 sensor_offsets C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sensor_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets)
-{
-	return mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, chan, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z);
-}
-
-/**
- * @brief Send a sensor_offsets message
- * @param chan MAVLink channel to send the message
- *
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @param mag_declination magnetic declination (radians)
- * @param raw_press raw pressure from barometer
- * @param raw_temp raw temperature from barometer
- * @param gyro_cal_x gyro X calibration
- * @param gyro_cal_y gyro Y calibration
- * @param gyro_cal_z gyro Z calibration
- * @param accel_cal_x accel X calibration
- * @param accel_cal_y accel Y calibration
- * @param accel_cal_z accel Z calibration
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_sensor_offsets_send(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[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN];
-	_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;
-	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
-
-// MESSAGE SENSOR_OFFSETS UNPACKING
-
-
-/**
- * @brief Get field mag_ofs_x from sensor_offsets message
- *
- * @return magnetometer X offset
- */
-static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  36);
-}
-
-/**
- * @brief Get field mag_ofs_y from sensor_offsets message
- *
- * @return magnetometer Y offset
- */
-static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  38);
-}
-
-/**
- * @brief Get field mag_ofs_z from sensor_offsets message
- *
- * @return magnetometer Z offset
- */
-static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  40);
-}
-
-/**
- * @brief Get field mag_declination from sensor_offsets message
- *
- * @return magnetic declination (radians)
- */
-static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field raw_press from sensor_offsets message
- *
- * @return raw pressure from barometer
- */
-static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field raw_temp from sensor_offsets message
- *
- * @return raw temperature from barometer
- */
-static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field gyro_cal_x from sensor_offsets message
- *
- * @return gyro X calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field gyro_cal_y from sensor_offsets message
- *
- * @return gyro Y calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field gyro_cal_z from sensor_offsets message
- *
- * @return gyro Z calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field accel_cal_x from sensor_offsets message
- *
- * @return accel X calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field accel_cal_y from sensor_offsets message
- *
- * @return accel Y calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field accel_cal_z from sensor_offsets message
- *
- * @return accel Z calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Decode a sensor_offsets message into a struct
- *
- * @param msg The message to decode
- * @param sensor_offsets C-struct to decode the message contents into
- */
-static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	sensor_offsets->mag_declination = mavlink_msg_sensor_offsets_get_mag_declination(msg);
-	sensor_offsets->raw_press = mavlink_msg_sensor_offsets_get_raw_press(msg);
-	sensor_offsets->raw_temp = mavlink_msg_sensor_offsets_get_raw_temp(msg);
-	sensor_offsets->gyro_cal_x = mavlink_msg_sensor_offsets_get_gyro_cal_x(msg);
-	sensor_offsets->gyro_cal_y = mavlink_msg_sensor_offsets_get_gyro_cal_y(msg);
-	sensor_offsets->gyro_cal_z = mavlink_msg_sensor_offsets_get_gyro_cal_z(msg);
-	sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg);
-	sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg);
-	sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg);
-	sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg);
-	sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg);
-	sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg);
-#else
-	memcpy(sensor_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-}
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
deleted file mode 100644
index b2c629c39..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE SET_MAG_OFFSETS PACKING
-
-#define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151
-
-typedef struct __mavlink_set_mag_offsets_t
-{
- int16_t mag_ofs_x; ///< magnetometer X offset
- int16_t mag_ofs_y; ///< magnetometer Y offset
- int16_t mag_ofs_z; ///< magnetometer Z offset
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_set_mag_offsets_t;
-
-#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN 8
-#define MAVLINK_MSG_ID_151_LEN 8
-
-#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC 219
-#define MAVLINK_MSG_ID_151_CRC 219
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS { \
-	"SET_MAG_OFFSETS", \
-	5, \
-	{  { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_set_mag_offsets_t, mag_ofs_x) }, \
-         { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_set_mag_offsets_t, mag_ofs_y) }, \
-         { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_set_mag_offsets_t, mag_ofs_z) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_set_mag_offsets_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_set_mag_offsets_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_mag_offsets 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 target_system System ID
- * @param target_component Component ID
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#else
-	mavlink_set_mag_offsets_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_mag_offsets 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 target_system System ID
- * @param target_component Component ID
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#else
-	mavlink_set_mag_offsets_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_mag_offsets 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 set_mag_offsets C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets)
-{
-	return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z);
-}
-
-/**
- * @brief Encode a set_mag_offsets 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 set_mag_offsets C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_mag_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets)
-{
-	return mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, chan, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z);
-}
-
-/**
- * @brief Send a set_mag_offsets message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_mag_offsets_send(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[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN];
-	_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;
-	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
-
-// MESSAGE SET_MAG_OFFSETS UNPACKING
-
-
-/**
- * @brief Get field target_system from set_mag_offsets message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field target_component from set_mag_offsets message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field mag_ofs_x from set_mag_offsets message
- *
- * @return magnetometer X offset
- */
-static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  0);
-}
-
-/**
- * @brief Get field mag_ofs_y from set_mag_offsets message
- *
- * @return magnetometer Y offset
- */
-static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  2);
-}
-
-/**
- * @brief Get field mag_ofs_z from set_mag_offsets message
- *
- * @return magnetometer Z offset
- */
-static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  4);
-}
-
-/**
- * @brief Decode a set_mag_offsets message into a struct
- *
- * @param msg The message to decode
- * @param set_mag_offsets C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg);
-	set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg);
-	set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg);
-	set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg);
-	set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg);
-#else
-	memcpy(set_mag_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h
deleted file mode 100644
index 8ef44f76d..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h
+++ /dev/null
@@ -1,397 +0,0 @@
-// MESSAGE SIMSTATE PACKING
-
-#define MAVLINK_MSG_ID_SIMSTATE 164
-
-typedef struct __mavlink_simstate_t
-{
- float roll; ///< Roll angle (rad)
- float pitch; ///< Pitch angle (rad)
- float yaw; ///< Yaw angle (rad)
- float xacc; ///< X acceleration m/s/s
- float yacc; ///< Y acceleration m/s/s
- float zacc; ///< Z acceleration m/s/s
- float xgyro; ///< Angular speed around X axis rad/s
- float ygyro; ///< Angular speed around Y axis rad/s
- float zgyro; ///< Angular speed around Z axis rad/s
- int32_t lat; ///< Latitude in degrees * 1E7
- int32_t lng; ///< Longitude in degrees * 1E7
-} mavlink_simstate_t;
-
-#define MAVLINK_MSG_ID_SIMSTATE_LEN 44
-#define MAVLINK_MSG_ID_164_LEN 44
-
-#define MAVLINK_MSG_ID_SIMSTATE_CRC 154
-#define MAVLINK_MSG_ID_164_CRC 154
-
-
-
-#define MAVLINK_MESSAGE_INFO_SIMSTATE { \
-	"SIMSTATE", \
-	11, \
-	{  { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_simstate_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_simstate_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_simstate_t, yaw) }, \
-         { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_simstate_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_simstate_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_simstate_t, zacc) }, \
-         { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_simstate_t, xgyro) }, \
-         { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_simstate_t, ygyro) }, \
-         { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_simstate_t, zgyro) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_simstate_t, lat) }, \
-         { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_simstate_t, lng) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a simstate 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 roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SIMSTATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#else
-	mavlink_simstate_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SIMSTATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a simstate 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 roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SIMSTATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#else
-	mavlink_simstate_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SIMSTATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a simstate 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 simstate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_simstate_t* simstate)
-{
-	return mavlink_msg_simstate_pack(system_id, component_id, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng);
-}
-
-/**
- * @brief Encode a simstate 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 simstate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_simstate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_simstate_t* simstate)
-{
-	return mavlink_msg_simstate_pack_chan(system_id, component_id, chan, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng);
-}
-
-/**
- * @brief Send a simstate message
- * @param chan MAVLink channel to send the message
- *
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_simstate_send(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[MAVLINK_MSG_ID_SIMSTATE_LEN];
-	_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;
-	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
-
-// MESSAGE SIMSTATE UNPACKING
-
-
-/**
- * @brief Get field roll from simstate message
- *
- * @return Roll angle (rad)
- */
-static inline float mavlink_msg_simstate_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field pitch from simstate message
- *
- * @return Pitch angle (rad)
- */
-static inline float mavlink_msg_simstate_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field yaw from simstate message
- *
- * @return Yaw angle (rad)
- */
-static inline float mavlink_msg_simstate_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field xacc from simstate message
- *
- * @return X acceleration m/s/s
- */
-static inline float mavlink_msg_simstate_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field yacc from simstate message
- *
- * @return Y acceleration m/s/s
- */
-static inline float mavlink_msg_simstate_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field zacc from simstate message
- *
- * @return Z acceleration m/s/s
- */
-static inline float mavlink_msg_simstate_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field xgyro from simstate message
- *
- * @return Angular speed around X axis rad/s
- */
-static inline float mavlink_msg_simstate_get_xgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field ygyro from simstate message
- *
- * @return Angular speed around Y axis rad/s
- */
-static inline float mavlink_msg_simstate_get_ygyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field zgyro from simstate message
- *
- * @return Angular speed around Z axis rad/s
- */
-static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field lat from simstate message
- *
- * @return Latitude in degrees * 1E7
- */
-static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  36);
-}
-
-/**
- * @brief Get field lng from simstate message
- *
- * @return Longitude in degrees * 1E7
- */
-static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  40);
-}
-
-/**
- * @brief Decode a simstate message into a struct
- *
- * @param msg The message to decode
- * @param simstate C-struct to decode the message contents into
- */
-static inline void mavlink_msg_simstate_decode(const mavlink_message_t* msg, mavlink_simstate_t* simstate)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	simstate->roll = mavlink_msg_simstate_get_roll(msg);
-	simstate->pitch = mavlink_msg_simstate_get_pitch(msg);
-	simstate->yaw = mavlink_msg_simstate_get_yaw(msg);
-	simstate->xacc = mavlink_msg_simstate_get_xacc(msg);
-	simstate->yacc = mavlink_msg_simstate_get_yacc(msg);
-	simstate->zacc = mavlink_msg_simstate_get_zacc(msg);
-	simstate->xgyro = mavlink_msg_simstate_get_xgyro(msg);
-	simstate->ygyro = mavlink_msg_simstate_get_ygyro(msg);
-	simstate->zgyro = mavlink_msg_simstate_get_zgyro(msg);
-	simstate->lat = mavlink_msg_simstate_get_lat(msg);
-	simstate->lng = mavlink_msg_simstate_get_lng(msg);
-#else
-	memcpy(simstate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h
deleted file mode 100644
index 7608a7bd1..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE WIND PACKING
-
-#define MAVLINK_MSG_ID_WIND 168
-
-typedef struct __mavlink_wind_t
-{
- float direction; ///< wind direction that wind is coming from (degrees)
- float speed; ///< wind speed in ground plane (m/s)
- float speed_z; ///< vertical wind speed (m/s)
-} mavlink_wind_t;
-
-#define MAVLINK_MSG_ID_WIND_LEN 12
-#define MAVLINK_MSG_ID_168_LEN 12
-
-#define MAVLINK_MSG_ID_WIND_CRC 1
-#define MAVLINK_MSG_ID_168_CRC 1
-
-
-
-#define MAVLINK_MESSAGE_INFO_WIND { \
-	"WIND", \
-	3, \
-	{  { "direction", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_wind_t, direction) }, \
-         { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_wind_t, speed) }, \
-         { "speed_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_wind_t, speed_z) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a wind 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 direction wind direction that wind is coming from (degrees)
- * @param speed wind speed in ground plane (m/s)
- * @param speed_z vertical wind speed (m/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float direction, float speed, float speed_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WIND_LEN];
-	_mav_put_float(buf, 0, direction);
-	_mav_put_float(buf, 4, speed);
-	_mav_put_float(buf, 8, speed_z);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN);
-#else
-	mavlink_wind_t packet;
-	packet.direction = direction;
-	packet.speed = speed;
-	packet.speed_z = speed_z;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WIND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-}
-
-/**
- * @brief Pack a wind 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 direction wind direction that wind is coming from (degrees)
- * @param speed wind speed in ground plane (m/s)
- * @param speed_z vertical wind speed (m/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float direction,float speed,float speed_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WIND_LEN];
-	_mav_put_float(buf, 0, direction);
-	_mav_put_float(buf, 4, speed);
-	_mav_put_float(buf, 8, speed_z);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN);
-#else
-	mavlink_wind_t packet;
-	packet.direction = direction;
-	packet.speed = speed;
-	packet.speed_z = speed_z;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WIND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-}
-
-/**
- * @brief Encode a wind 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 wind C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wind_t* wind)
-{
-	return mavlink_msg_wind_pack(system_id, component_id, msg, wind->direction, wind->speed, wind->speed_z);
-}
-
-/**
- * @brief Encode a wind 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 wind C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wind_t* wind)
-{
-	return mavlink_msg_wind_pack_chan(system_id, component_id, chan, msg, wind->direction, wind->speed, wind->speed_z);
-}
-
-/**
- * @brief Send a wind message
- * @param chan MAVLink channel to send the message
- *
- * @param direction wind direction that wind is coming from (degrees)
- * @param speed wind speed in ground plane (m/s)
- * @param speed_z vertical wind speed (m/s)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_wind_send(mavlink_channel_t chan, float direction, float speed, float speed_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WIND_LEN];
-	_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;
-	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
-
-// MESSAGE WIND UNPACKING
-
-
-/**
- * @brief Get field direction from wind message
- *
- * @return wind direction that wind is coming from (degrees)
- */
-static inline float mavlink_msg_wind_get_direction(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field speed from wind message
- *
- * @return wind speed in ground plane (m/s)
- */
-static inline float mavlink_msg_wind_get_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field speed_z from wind message
- *
- * @return vertical wind speed (m/s)
- */
-static inline float mavlink_msg_wind_get_speed_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Decode a wind message into a struct
- *
- * @param msg The message to decode
- * @param wind C-struct to decode the message contents into
- */
-static inline void mavlink_msg_wind_decode(const mavlink_message_t* msg, mavlink_wind_t* wind)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	wind->direction = mavlink_msg_wind_get_direction(msg);
-	wind->speed = mavlink_msg_wind_get_speed(msg);
-	wind->speed_z = mavlink_msg_wind_get_speed_z(msg);
-#else
-	memcpy(wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WIND_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
deleted file mode 100644
index b2496334f..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
+++ /dev/null
@@ -1,1488 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from ardupilotmega.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef ARDUPILOTMEGA_TESTSUITE_H
-#define ARDUPILOTMEGA_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg);
-static void mavlink_test_ardupilotmega(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_common(system_id, component_id, last_msg);
-	mavlink_test_ardupilotmega(system_id, component_id, last_msg);
-}
-#endif
-
-#include "../common/testsuite.h"
-
-
-static void mavlink_test_sensor_offsets(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_sensor_offsets_t packet_in = {
-		17.0,
-	}963497672,
-	}963497880,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}19107,
-	}19211,
-	}19315,
-	};
-	mavlink_sensor_offsets_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.mag_declination = packet_in.mag_declination;
-        	packet1.raw_press = packet_in.raw_press;
-        	packet1.raw_temp = packet_in.raw_temp;
-        	packet1.gyro_cal_x = packet_in.gyro_cal_x;
-        	packet1.gyro_cal_y = packet_in.gyro_cal_y;
-        	packet1.gyro_cal_z = packet_in.gyro_cal_z;
-        	packet1.accel_cal_x = packet_in.accel_cal_x;
-        	packet1.accel_cal_y = packet_in.accel_cal_y;
-        	packet1.accel_cal_z = packet_in.accel_cal_z;
-        	packet1.mag_ofs_x = packet_in.mag_ofs_x;
-        	packet1.mag_ofs_y = packet_in.mag_ofs_y;
-        	packet1.mag_ofs_z = packet_in.mag_ofs_z;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_sensor_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z );
-	mavlink_msg_sensor_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z );
-	mavlink_msg_sensor_offsets_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_sensor_offsets_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sensor_offsets_send(MAVLINK_COMM_1 , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z );
-	mavlink_msg_sensor_offsets_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_mag_offsets(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_set_mag_offsets_t packet_in = {
-		17235,
-	}17339,
-	}17443,
-	}151,
-	}218,
-	};
-	mavlink_set_mag_offsets_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.mag_ofs_x = packet_in.mag_ofs_x;
-        	packet1.mag_ofs_y = packet_in.mag_ofs_y;
-        	packet1.mag_ofs_z = packet_in.mag_ofs_z;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mag_offsets_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_mag_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mag_offsets_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
-	mavlink_msg_set_mag_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
-	mavlink_msg_set_mag_offsets_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_set_mag_offsets_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mag_offsets_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
-	mavlink_msg_set_mag_offsets_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_meminfo(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_meminfo_t packet_in = {
-		17235,
-	}17339,
-	};
-	mavlink_meminfo_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.brkval = packet_in.brkval;
-        	packet1.freemem = packet_in.freemem;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_meminfo_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_meminfo_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_meminfo_pack(system_id, component_id, &msg , packet1.brkval , packet1.freemem );
-	mavlink_msg_meminfo_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_meminfo_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.brkval , packet1.freemem );
-	mavlink_msg_meminfo_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_meminfo_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_meminfo_send(MAVLINK_COMM_1 , packet1.brkval , packet1.freemem );
-	mavlink_msg_meminfo_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_ap_adc(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_ap_adc_t packet_in = {
-		17235,
-	}17339,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	};
-	mavlink_ap_adc_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.adc1 = packet_in.adc1;
-        	packet1.adc2 = packet_in.adc2;
-        	packet1.adc3 = packet_in.adc3;
-        	packet1.adc4 = packet_in.adc4;
-        	packet1.adc5 = packet_in.adc5;
-        	packet1.adc6 = packet_in.adc6;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_ap_adc_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ap_adc_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 );
-	mavlink_msg_ap_adc_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ap_adc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 );
-	mavlink_msg_ap_adc_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_ap_adc_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ap_adc_send(MAVLINK_COMM_1 , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 );
-	mavlink_msg_ap_adc_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_digicam_configure(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_digicam_configure_t packet_in = {
-		17.0,
-	}17443,
-	}151,
-	}218,
-	}29,
-	}96,
-	}163,
-	}230,
-	}41,
-	}108,
-	}175,
-	};
-	mavlink_digicam_configure_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.extra_value = packet_in.extra_value;
-        	packet1.shutter_speed = packet_in.shutter_speed;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.mode = packet_in.mode;
-        	packet1.aperture = packet_in.aperture;
-        	packet1.iso = packet_in.iso;
-        	packet1.exposure_type = packet_in.exposure_type;
-        	packet1.command_id = packet_in.command_id;
-        	packet1.engine_cut_off = packet_in.engine_cut_off;
-        	packet1.extra_param = packet_in.extra_param;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_configure_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_digicam_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_configure_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mode , packet1.shutter_speed , packet1.aperture , packet1.iso , packet1.exposure_type , packet1.command_id , packet1.engine_cut_off , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_configure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mode , packet1.shutter_speed , packet1.aperture , packet1.iso , packet1.exposure_type , packet1.command_id , packet1.engine_cut_off , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_configure_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_digicam_configure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_configure_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mode , packet1.shutter_speed , packet1.aperture , packet1.iso , packet1.exposure_type , packet1.command_id , packet1.engine_cut_off , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_configure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_digicam_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_digicam_control_t packet_in = {
-		17.0,
-	}17,
-	}84,
-	}151,
-	}218,
-	}29,
-	}96,
-	}163,
-	}230,
-	}41,
-	};
-	mavlink_digicam_control_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.extra_value = packet_in.extra_value;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.session = packet_in.session;
-        	packet1.zoom_pos = packet_in.zoom_pos;
-        	packet1.zoom_step = packet_in.zoom_step;
-        	packet1.focus_lock = packet_in.focus_lock;
-        	packet1.shot = packet_in.shot;
-        	packet1.command_id = packet_in.command_id;
-        	packet1.extra_param = packet_in.extra_param;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_control_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_digicam_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_control_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.session , packet1.zoom_pos , packet1.zoom_step , packet1.focus_lock , packet1.shot , packet1.command_id , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.session , packet1.zoom_pos , packet1.zoom_step , packet1.focus_lock , packet1.shot , packet1.command_id , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_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_digicam_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_control_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.session , packet1.zoom_pos , packet1.zoom_step , packet1.focus_lock , packet1.shot , packet1.command_id , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mount_configure(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_mount_configure_t packet_in = {
-		5,
-	}72,
-	}139,
-	}206,
-	}17,
-	}84,
-	};
-	mavlink_mount_configure_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.mount_mode = packet_in.mount_mode;
-        	packet1.stab_roll = packet_in.stab_roll;
-        	packet1.stab_pitch = packet_in.stab_pitch;
-        	packet1.stab_yaw = packet_in.stab_yaw;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_configure_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mount_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_configure_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mount_mode , packet1.stab_roll , packet1.stab_pitch , packet1.stab_yaw );
-	mavlink_msg_mount_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_configure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mount_mode , packet1.stab_roll , packet1.stab_pitch , packet1.stab_yaw );
-	mavlink_msg_mount_configure_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_mount_configure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_configure_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mount_mode , packet1.stab_roll , packet1.stab_pitch , packet1.stab_yaw );
-	mavlink_msg_mount_configure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mount_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_mount_control_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}41,
-	}108,
-	}175,
-	};
-	mavlink_mount_control_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.input_a = packet_in.input_a;
-        	packet1.input_b = packet_in.input_b;
-        	packet1.input_c = packet_in.input_c;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.save_position = packet_in.save_position;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_control_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mount_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_control_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.input_a , packet1.input_b , packet1.input_c , packet1.save_position );
-	mavlink_msg_mount_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.input_a , packet1.input_b , packet1.input_c , packet1.save_position );
-	mavlink_msg_mount_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_mount_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_control_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.input_a , packet1.input_b , packet1.input_c , packet1.save_position );
-	mavlink_msg_mount_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mount_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_mount_status_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}41,
-	}108,
-	};
-	mavlink_mount_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.pointing_a = packet_in.pointing_a;
-        	packet1.pointing_b = packet_in.pointing_b;
-        	packet1.pointing_c = packet_in.pointing_c;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mount_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_status_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.pointing_a , packet1.pointing_b , packet1.pointing_c );
-	mavlink_msg_mount_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.pointing_a , packet1.pointing_b , packet1.pointing_c );
-	mavlink_msg_mount_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_mount_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_status_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.pointing_a , packet1.pointing_b , packet1.pointing_c );
-	mavlink_msg_mount_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_fence_point(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_fence_point_t packet_in = {
-		17.0,
-	}45.0,
-	}29,
-	}96,
-	}163,
-	}230,
-	};
-	mavlink_fence_point_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.lat = packet_in.lat;
-        	packet1.lng = packet_in.lng;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.idx = packet_in.idx;
-        	packet1.count = packet_in.count;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_point_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_fence_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng );
-	mavlink_msg_fence_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng );
-	mavlink_msg_fence_point_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_fence_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_point_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng );
-	mavlink_msg_fence_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_fence_fetch_point(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_fence_fetch_point_t packet_in = {
-		5,
-	}72,
-	}139,
-	};
-	mavlink_fence_fetch_point_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.idx = packet_in.idx;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_fetch_point_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_fence_fetch_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_fetch_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_fence_fetch_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_fence_fetch_point_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_fence_fetch_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_fetch_point_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_fence_fetch_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_fence_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_fence_status_t packet_in = {
-		963497464,
-	}17443,
-	}151,
-	}218,
-	};
-	mavlink_fence_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.breach_time = packet_in.breach_time;
-        	packet1.breach_count = packet_in.breach_count;
-        	packet1.breach_status = packet_in.breach_status;
-        	packet1.breach_type = packet_in.breach_type;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_fence_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_status_pack(system_id, component_id, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time );
-	mavlink_msg_fence_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time );
-	mavlink_msg_fence_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_fence_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_status_send(MAVLINK_COMM_1 , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time );
-	mavlink_msg_fence_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_ahrs(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_ahrs_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	};
-	mavlink_ahrs_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.omegaIx = packet_in.omegaIx;
-        	packet1.omegaIy = packet_in.omegaIy;
-        	packet1.omegaIz = packet_in.omegaIz;
-        	packet1.accel_weight = packet_in.accel_weight;
-        	packet1.renorm_val = packet_in.renorm_val;
-        	packet1.error_rp = packet_in.error_rp;
-        	packet1.error_yaw = packet_in.error_yaw;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_ahrs_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_ahrs_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_ahrs_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_ahrs_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_ahrs_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_simstate(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_simstate_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}963499336,
-	}963499544,
-	};
-	mavlink_simstate_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        	packet1.xgyro = packet_in.xgyro;
-        	packet1.ygyro = packet_in.ygyro;
-        	packet1.zgyro = packet_in.zgyro;
-        	packet1.lat = packet_in.lat;
-        	packet1.lng = packet_in.lng;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_simstate_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_simstate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_simstate_pack(system_id, component_id, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lng );
-	mavlink_msg_simstate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_simstate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lng );
-	mavlink_msg_simstate_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_simstate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_simstate_send(MAVLINK_COMM_1 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lng );
-	mavlink_msg_simstate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hwstatus(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_hwstatus_t packet_in = {
-		17235,
-	}139,
-	};
-	mavlink_hwstatus_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.Vcc = packet_in.Vcc;
-        	packet1.I2Cerr = packet_in.I2Cerr;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hwstatus_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hwstatus_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hwstatus_pack(system_id, component_id, &msg , packet1.Vcc , packet1.I2Cerr );
-	mavlink_msg_hwstatus_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hwstatus_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Vcc , packet1.I2Cerr );
-	mavlink_msg_hwstatus_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_hwstatus_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hwstatus_send(MAVLINK_COMM_1 , packet1.Vcc , packet1.I2Cerr );
-	mavlink_msg_hwstatus_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_radio(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_radio_t packet_in = {
-		17235,
-	}17339,
-	}17,
-	}84,
-	}151,
-	}218,
-	}29,
-	};
-	mavlink_radio_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.rxerrors = packet_in.rxerrors;
-        	packet1.fixed = packet_in.fixed;
-        	packet1.rssi = packet_in.rssi;
-        	packet1.remrssi = packet_in.remrssi;
-        	packet1.txbuf = packet_in.txbuf;
-        	packet1.noise = packet_in.noise;
-        	packet1.remnoise = packet_in.remnoise;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_radio_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_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_radio_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_send(MAVLINK_COMM_1 , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_limits_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_limits_status_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}963498088,
-	}18067,
-	}187,
-	}254,
-	}65,
-	}132,
-	};
-	mavlink_limits_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.last_trigger = packet_in.last_trigger;
-        	packet1.last_action = packet_in.last_action;
-        	packet1.last_recovery = packet_in.last_recovery;
-        	packet1.last_clear = packet_in.last_clear;
-        	packet1.breach_count = packet_in.breach_count;
-        	packet1.limits_state = packet_in.limits_state;
-        	packet1.mods_enabled = packet_in.mods_enabled;
-        	packet1.mods_required = packet_in.mods_required;
-        	packet1.mods_triggered = packet_in.mods_triggered;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_limits_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_limits_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_limits_status_pack(system_id, component_id, &msg , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered );
-	mavlink_msg_limits_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_limits_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered );
-	mavlink_msg_limits_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_limits_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_limits_status_send(MAVLINK_COMM_1 , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered );
-	mavlink_msg_limits_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_wind(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_wind_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	};
-	mavlink_wind_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.direction = packet_in.direction;
-        	packet1.speed = packet_in.speed;
-        	packet1.speed_z = packet_in.speed_z;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_wind_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_wind_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_wind_pack(system_id, component_id, &msg , packet1.direction , packet1.speed , packet1.speed_z );
-	mavlink_msg_wind_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_wind_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.direction , packet1.speed , packet1.speed_z );
-	mavlink_msg_wind_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_wind_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_wind_send(MAVLINK_COMM_1 , packet1.direction , packet1.speed , packet1.speed_z );
-	mavlink_msg_wind_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data16(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_data16_t packet_in = {
-		5,
-	}72,
-	}{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154 },
-	};
-	mavlink_data16_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.type = packet_in.type;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*16);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data16_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data16_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data16_pack(system_id, component_id, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data16_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data16_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data16_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_data16_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data16_send(MAVLINK_COMM_1 , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data16_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data32(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_data32_t packet_in = {
-		5,
-	}72,
-	}{ 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, 166, 167, 168, 169, 170 },
-	};
-	mavlink_data32_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.type = packet_in.type;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*32);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data32_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data32_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data32_pack(system_id, component_id, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data32_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data32_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data32_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_data32_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data32_send(MAVLINK_COMM_1 , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data32_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data64(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_data64_t packet_in = {
-		5,
-	}72,
-	}{ 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, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 },
-	};
-	mavlink_data64_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.type = packet_in.type;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*64);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data64_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data64_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data64_pack(system_id, component_id, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data64_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data64_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data64_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_data64_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data64_send(MAVLINK_COMM_1 , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data64_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data96(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_data96_t packet_in = {
-		5,
-	}72,
-	}{ 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, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234 },
-	};
-	mavlink_data96_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.type = packet_in.type;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*96);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data96_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data96_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data96_pack(system_id, component_id, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data96_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data96_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data96_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_data96_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data96_send(MAVLINK_COMM_1 , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data96_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_rangefinder(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_rangefinder_t packet_in = {
-		17.0,
-	}45.0,
-	};
-	mavlink_rangefinder_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.distance = packet_in.distance;
-        	packet1.voltage = packet_in.voltage;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rangefinder_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_rangefinder_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rangefinder_pack(system_id, component_id, &msg , packet1.distance , packet1.voltage );
-	mavlink_msg_rangefinder_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rangefinder_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.distance , packet1.voltage );
-	mavlink_msg_rangefinder_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_rangefinder_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rangefinder_send(MAVLINK_COMM_1 , packet1.distance , packet1.voltage );
-	mavlink_msg_rangefinder_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_airspeed_autocal(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_airspeed_autocal_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}269.0,
-	}297.0,
-	}325.0,
-	};
-	mavlink_airspeed_autocal_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.vx = packet_in.vx;
-        	packet1.vy = packet_in.vy;
-        	packet1.vz = packet_in.vz;
-        	packet1.diff_pressure = packet_in.diff_pressure;
-        	packet1.EAS2TAS = packet_in.EAS2TAS;
-        	packet1.ratio = packet_in.ratio;
-        	packet1.state_x = packet_in.state_x;
-        	packet1.state_y = packet_in.state_y;
-        	packet1.state_z = packet_in.state_z;
-        	packet1.Pax = packet_in.Pax;
-        	packet1.Pby = packet_in.Pby;
-        	packet1.Pcz = packet_in.Pcz;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_airspeed_autocal_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_airspeed_autocal_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_airspeed_autocal_pack(system_id, component_id, &msg , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz );
-	mavlink_msg_airspeed_autocal_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz );
-	mavlink_msg_airspeed_autocal_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_airspeed_autocal_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_airspeed_autocal_send(MAVLINK_COMM_1 , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz );
-	mavlink_msg_airspeed_autocal_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_rally_point(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_rally_point_t packet_in = {
-		963497464,
-	}963497672,
-	}17651,
-	}17755,
-	}17859,
-	}175,
-	}242,
-	}53,
-	}120,
-	}187,
-	};
-	mavlink_rally_point_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.lat = packet_in.lat;
-        	packet1.lng = packet_in.lng;
-        	packet1.alt = packet_in.alt;
-        	packet1.break_alt = packet_in.break_alt;
-        	packet1.land_dir = packet_in.land_dir;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.idx = packet_in.idx;
-        	packet1.count = packet_in.count;
-        	packet1.flags = packet_in.flags;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rally_point_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_rally_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rally_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng , packet1.alt , packet1.break_alt , packet1.land_dir , packet1.flags );
-	mavlink_msg_rally_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rally_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng , packet1.alt , packet1.break_alt , packet1.land_dir , packet1.flags );
-	mavlink_msg_rally_point_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_rally_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rally_point_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng , packet1.alt , packet1.break_alt , packet1.land_dir , packet1.flags );
-	mavlink_msg_rally_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_rally_fetch_point(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_rally_fetch_point_t packet_in = {
-		5,
-	}72,
-	}139,
-	};
-	mavlink_rally_fetch_point_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.idx = packet_in.idx;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rally_fetch_point_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_rally_fetch_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rally_fetch_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_rally_fetch_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rally_fetch_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_rally_fetch_point_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_rally_fetch_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rally_fetch_point_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_rally_fetch_point_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;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_ahrs2_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}963498296,
-	}963498504,
-	};
-	mavlink_ahrs2_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.altitude = packet_in.altitude;
-        	packet1.lat = packet_in.lat;
-        	packet1.lng = packet_in.lng;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs2_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_ahrs2_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs2_pack(system_id, component_id, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
-	mavlink_msg_ahrs2_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
-	mavlink_msg_ahrs2_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_ahrs2_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs2_send(MAVLINK_COMM_1 , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
-	mavlink_msg_ahrs2_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_sensor_offsets(system_id, component_id, last_msg);
-	mavlink_test_set_mag_offsets(system_id, component_id, last_msg);
-	mavlink_test_meminfo(system_id, component_id, last_msg);
-	mavlink_test_ap_adc(system_id, component_id, last_msg);
-	mavlink_test_digicam_configure(system_id, component_id, last_msg);
-	mavlink_test_digicam_control(system_id, component_id, last_msg);
-	mavlink_test_mount_configure(system_id, component_id, last_msg);
-	mavlink_test_mount_control(system_id, component_id, last_msg);
-	mavlink_test_mount_status(system_id, component_id, last_msg);
-	mavlink_test_fence_point(system_id, component_id, last_msg);
-	mavlink_test_fence_fetch_point(system_id, component_id, last_msg);
-	mavlink_test_fence_status(system_id, component_id, last_msg);
-	mavlink_test_ahrs(system_id, component_id, last_msg);
-	mavlink_test_simstate(system_id, component_id, last_msg);
-	mavlink_test_hwstatus(system_id, component_id, last_msg);
-	mavlink_test_radio(system_id, component_id, last_msg);
-	mavlink_test_limits_status(system_id, component_id, last_msg);
-	mavlink_test_wind(system_id, component_id, last_msg);
-	mavlink_test_data16(system_id, component_id, last_msg);
-	mavlink_test_data32(system_id, component_id, last_msg);
-	mavlink_test_data64(system_id, component_id, last_msg);
-	mavlink_test_data96(system_id, component_id, last_msg);
-	mavlink_test_rangefinder(system_id, component_id, last_msg);
-	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_ahrs2(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // ARDUPILOTMEGA_TESTSUITE_H
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h
deleted file mode 100644
index 983237eb5..000000000
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from ardupilotmega.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:27:33 2014"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
- 
-#endif // MAVLINK_VERSION_H
diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h
deleted file mode 100644
index b4dde669f..000000000
--- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h
+++ /dev/null
@@ -1,123 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol generated from autoquad.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef AUTOQUAD_H
-#define AUTOQUAD_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// 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}
-#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}
-#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}}}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_AUTOQUAD
-
-// ENUM DEFINITIONS
-
-
-/** @brief Available operating modes/statuses for AutoQuad flight controller. 
-				Bitmask up to 32 bits. Low side bits for base modes, high side for 
-				additional active features/modifiers/constraints. */
-#ifndef HAVE_ENUM_AUTOQUAD_NAV_STATUS
-#define HAVE_ENUM_AUTOQUAD_NAV_STATUS
-enum AUTOQUAD_NAV_STATUS
-{
-	AQ_NAV_STATUS_INIT=0, /* System is initializing | */
-	AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */
-	AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */
-	AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */
-	AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */
-	AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */
-	AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */
-	AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */
-	AQ_NAV_STATUS_CEILING=134217728, /* Ceiling altitude is set | */
-	AQ_NAV_STATUS_HF_DYNAMIC=268435456, /* Heading-Free dynamic mode active | */
-	AQ_NAV_STATUS_HF_LOCKED=536870912, /* Heading-Free locked mode active | */
-	AQ_NAV_STATUS_RTH=1073741824, /* Automatic Return to Home is active | */
-	AQ_NAV_STATUS_FAILSAFE=2147483648, /* System is in failsafe recovery mode | */
-	AUTOQUAD_NAV_STATUS_ENUM_END=2147483649, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_CMD
-#define HAVE_ENUM_MAV_CMD
-enum MAV_CMD
-{
-	MAV_CMD_AQ_TELEMETRY=2, /* Start/stop AutoQuad telemetry values stream. |Start or stop (1 or 0)| Stream frequency in us| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_AQ_FOLLOW=3, /* Command AutoQuad to go to a particular place at a set speed. |Latitude| Lontitude| Altitude| Speed| Empty| Empty| Empty|  */
-	MAV_CMD_AQ_REQUEST_VERSION=4, /* Request AutoQuad firmware version number. |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	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_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
-
-#include "../common/common.h"
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_aq_telemetry_f.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // AUTOQUAD_H
diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink.h
deleted file mode 100644
index 3f80c9a41..000000000
--- a/mavlink/include/mavlink/v1.0/autoquad/mavlink.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from autoquad.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "autoquad.h"
-
-#endif // MAVLINK_H
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
deleted file mode 100644
index ed7c86bcb..000000000
--- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h
+++ /dev/null
@@ -1,617 +0,0 @@
-// MESSAGE AQ_TELEMETRY_F PACKING
-
-#define MAVLINK_MSG_ID_AQ_TELEMETRY_F 150
-
-typedef struct __mavlink_aq_telemetry_f_t
-{
- float value1; ///< value1
- float value2; ///< value2
- float value3; ///< value3
- float value4; ///< value4
- float value5; ///< value5
- float value6; ///< value6
- float value7; ///< value7
- float value8; ///< value8
- float value9; ///< value9
- float value10; ///< value10
- float value11; ///< value11
- float value12; ///< value12
- float value13; ///< value13
- float value14; ///< value14
- float value15; ///< value15
- float value16; ///< value16
- float value17; ///< value17
- float value18; ///< value18
- float value19; ///< value19
- float value20; ///< value20
- uint16_t Index; ///< Index of message
-} mavlink_aq_telemetry_f_t;
-
-#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN 82
-#define MAVLINK_MSG_ID_150_LEN 82
-
-#define MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC 241
-#define MAVLINK_MSG_ID_150_CRC 241
-
-
-
-#define MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F { \
-	"AQ_TELEMETRY_F", \
-	21, \
-	{  { "value1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_aq_telemetry_f_t, value1) }, \
-         { "value2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_aq_telemetry_f_t, value2) }, \
-         { "value3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_aq_telemetry_f_t, value3) }, \
-         { "value4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_aq_telemetry_f_t, value4) }, \
-         { "value5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_aq_telemetry_f_t, value5) }, \
-         { "value6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_aq_telemetry_f_t, value6) }, \
-         { "value7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_aq_telemetry_f_t, value7) }, \
-         { "value8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_aq_telemetry_f_t, value8) }, \
-         { "value9", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_aq_telemetry_f_t, value9) }, \
-         { "value10", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_aq_telemetry_f_t, value10) }, \
-         { "value11", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_aq_telemetry_f_t, value11) }, \
-         { "value12", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_aq_telemetry_f_t, value12) }, \
-         { "value13", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_aq_telemetry_f_t, value13) }, \
-         { "value14", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_aq_telemetry_f_t, value14) }, \
-         { "value15", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_aq_telemetry_f_t, value15) }, \
-         { "value16", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_aq_telemetry_f_t, value16) }, \
-         { "value17", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_aq_telemetry_f_t, value17) }, \
-         { "value18", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_aq_telemetry_f_t, value18) }, \
-         { "value19", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_aq_telemetry_f_t, value19) }, \
-         { "value20", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_aq_telemetry_f_t, value20) }, \
-         { "Index", NULL, MAVLINK_TYPE_UINT16_T, 0, 80, offsetof(mavlink_aq_telemetry_f_t, Index) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a aq_telemetry_f 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 Index Index of message
- * @param value1 value1
- * @param value2 value2
- * @param value3 value3
- * @param value4 value4
- * @param value5 value5
- * @param value6 value6
- * @param value7 value7
- * @param value8 value8
- * @param value9 value9
- * @param value10 value10
- * @param value11 value11
- * @param value12 value12
- * @param value13 value13
- * @param value14 value14
- * @param value15 value15
- * @param value16 value16
- * @param value17 value17
- * @param value18 value18
- * @param value19 value19
- * @param value20 value20
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_aq_telemetry_f_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
-#else
-	mavlink_aq_telemetry_f_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
-#endif
-}
-
-/**
- * @brief Pack a aq_telemetry_f 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 Index Index of message
- * @param value1 value1
- * @param value2 value2
- * @param value3 value3
- * @param value4 value4
- * @param value5 value5
- * @param value6 value6
- * @param value7 value7
- * @param value8 value8
- * @param value9 value9
- * @param value10 value10
- * @param value11 value11
- * @param value12 value12
- * @param value13 value13
- * @param value14 value14
- * @param value15 value15
- * @param value16 value16
- * @param value17 value17
- * @param value18 value18
- * @param value19 value19
- * @param value20 value20
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_aq_telemetry_f_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
-#else
-	mavlink_aq_telemetry_f_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AQ_TELEMETRY_F;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
-#endif
-}
-
-/**
- * @brief Encode a aq_telemetry_f 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 aq_telemetry_f C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_aq_telemetry_f_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f)
-{
-	return mavlink_msg_aq_telemetry_f_pack(system_id, component_id, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20);
-}
-
-/**
- * @brief Encode a aq_telemetry_f 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 aq_telemetry_f C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_aq_telemetry_f_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f)
-{
-	return mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, chan, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20);
-}
-
-/**
- * @brief Send a aq_telemetry_f message
- * @param chan MAVLink channel to send the message
- *
- * @param Index Index of message
- * @param value1 value1
- * @param value2 value2
- * @param value3 value3
- * @param value4 value4
- * @param value5 value5
- * @param value6 value6
- * @param value7 value7
- * @param value8 value8
- * @param value9 value9
- * @param value10 value10
- * @param value11 value11
- * @param value12 value12
- * @param value13 value13
- * @param value14 value14
- * @param value15 value15
- * @param value16 value16
- * @param value17 value17
- * @param value18 value18
- * @param value19 value19
- * @param value20 value20
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_aq_telemetry_f_send(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[MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN];
-	_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;
-	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
-
-// MESSAGE AQ_TELEMETRY_F UNPACKING
-
-
-/**
- * @brief Get field Index from aq_telemetry_f message
- *
- * @return Index of message
- */
-static inline uint16_t mavlink_msg_aq_telemetry_f_get_Index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  80);
-}
-
-/**
- * @brief Get field value1 from aq_telemetry_f message
- *
- * @return value1
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field value2 from aq_telemetry_f message
- *
- * @return value2
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field value3 from aq_telemetry_f message
- *
- * @return value3
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field value4 from aq_telemetry_f message
- *
- * @return value4
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field value5 from aq_telemetry_f message
- *
- * @return value5
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field value6 from aq_telemetry_f message
- *
- * @return value6
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field value7 from aq_telemetry_f message
- *
- * @return value7
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value7(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field value8 from aq_telemetry_f message
- *
- * @return value8
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field value9 from aq_telemetry_f message
- *
- * @return value9
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value9(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field value10 from aq_telemetry_f message
- *
- * @return value10
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value10(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  36);
-}
-
-/**
- * @brief Get field value11 from aq_telemetry_f message
- *
- * @return value11
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value11(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  40);
-}
-
-/**
- * @brief Get field value12 from aq_telemetry_f message
- *
- * @return value12
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value12(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  44);
-}
-
-/**
- * @brief Get field value13 from aq_telemetry_f message
- *
- * @return value13
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value13(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  48);
-}
-
-/**
- * @brief Get field value14 from aq_telemetry_f message
- *
- * @return value14
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value14(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  52);
-}
-
-/**
- * @brief Get field value15 from aq_telemetry_f message
- *
- * @return value15
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value15(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  56);
-}
-
-/**
- * @brief Get field value16 from aq_telemetry_f message
- *
- * @return value16
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value16(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  60);
-}
-
-/**
- * @brief Get field value17 from aq_telemetry_f message
- *
- * @return value17
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value17(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  64);
-}
-
-/**
- * @brief Get field value18 from aq_telemetry_f message
- *
- * @return value18
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value18(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  68);
-}
-
-/**
- * @brief Get field value19 from aq_telemetry_f message
- *
- * @return value19
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value19(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  72);
-}
-
-/**
- * @brief Get field value20 from aq_telemetry_f message
- *
- * @return value20
- */
-static inline float mavlink_msg_aq_telemetry_f_get_value20(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  76);
-}
-
-/**
- * @brief Decode a aq_telemetry_f message into a struct
- *
- * @param msg The message to decode
- * @param aq_telemetry_f C-struct to decode the message contents into
- */
-static inline void mavlink_msg_aq_telemetry_f_decode(const mavlink_message_t* msg, mavlink_aq_telemetry_f_t* aq_telemetry_f)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	aq_telemetry_f->value1 = mavlink_msg_aq_telemetry_f_get_value1(msg);
-	aq_telemetry_f->value2 = mavlink_msg_aq_telemetry_f_get_value2(msg);
-	aq_telemetry_f->value3 = mavlink_msg_aq_telemetry_f_get_value3(msg);
-	aq_telemetry_f->value4 = mavlink_msg_aq_telemetry_f_get_value4(msg);
-	aq_telemetry_f->value5 = mavlink_msg_aq_telemetry_f_get_value5(msg);
-	aq_telemetry_f->value6 = mavlink_msg_aq_telemetry_f_get_value6(msg);
-	aq_telemetry_f->value7 = mavlink_msg_aq_telemetry_f_get_value7(msg);
-	aq_telemetry_f->value8 = mavlink_msg_aq_telemetry_f_get_value8(msg);
-	aq_telemetry_f->value9 = mavlink_msg_aq_telemetry_f_get_value9(msg);
-	aq_telemetry_f->value10 = mavlink_msg_aq_telemetry_f_get_value10(msg);
-	aq_telemetry_f->value11 = mavlink_msg_aq_telemetry_f_get_value11(msg);
-	aq_telemetry_f->value12 = mavlink_msg_aq_telemetry_f_get_value12(msg);
-	aq_telemetry_f->value13 = mavlink_msg_aq_telemetry_f_get_value13(msg);
-	aq_telemetry_f->value14 = mavlink_msg_aq_telemetry_f_get_value14(msg);
-	aq_telemetry_f->value15 = mavlink_msg_aq_telemetry_f_get_value15(msg);
-	aq_telemetry_f->value16 = mavlink_msg_aq_telemetry_f_get_value16(msg);
-	aq_telemetry_f->value17 = mavlink_msg_aq_telemetry_f_get_value17(msg);
-	aq_telemetry_f->value18 = mavlink_msg_aq_telemetry_f_get_value18(msg);
-	aq_telemetry_f->value19 = mavlink_msg_aq_telemetry_f_get_value19(msg);
-	aq_telemetry_f->value20 = mavlink_msg_aq_telemetry_f_get_value20(msg);
-	aq_telemetry_f->Index = mavlink_msg_aq_telemetry_f_get_Index(msg);
-#else
-	memcpy(aq_telemetry_f, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h b/mavlink/include/mavlink/v1.0/autoquad/testsuite.h
deleted file mode 100644
index 4eafe7be5..000000000
--- a/mavlink/include/mavlink/v1.0/autoquad/testsuite.h
+++ /dev/null
@@ -1,118 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from autoquad.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef AUTOQUAD_TESTSUITE_H
-#define AUTOQUAD_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg);
-static void mavlink_test_autoquad(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_common(system_id, component_id, last_msg);
-	mavlink_test_autoquad(system_id, component_id, last_msg);
-}
-#endif
-
-#include "../common/testsuite.h"
-
-
-static void mavlink_test_aq_telemetry_f(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_aq_telemetry_f_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}269.0,
-	}297.0,
-	}325.0,
-	}353.0,
-	}381.0,
-	}409.0,
-	}437.0,
-	}465.0,
-	}493.0,
-	}521.0,
-	}549.0,
-	}21395,
-	};
-	mavlink_aq_telemetry_f_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.value1 = packet_in.value1;
-        	packet1.value2 = packet_in.value2;
-        	packet1.value3 = packet_in.value3;
-        	packet1.value4 = packet_in.value4;
-        	packet1.value5 = packet_in.value5;
-        	packet1.value6 = packet_in.value6;
-        	packet1.value7 = packet_in.value7;
-        	packet1.value8 = packet_in.value8;
-        	packet1.value9 = packet_in.value9;
-        	packet1.value10 = packet_in.value10;
-        	packet1.value11 = packet_in.value11;
-        	packet1.value12 = packet_in.value12;
-        	packet1.value13 = packet_in.value13;
-        	packet1.value14 = packet_in.value14;
-        	packet1.value15 = packet_in.value15;
-        	packet1.value16 = packet_in.value16;
-        	packet1.value17 = packet_in.value17;
-        	packet1.value18 = packet_in.value18;
-        	packet1.value19 = packet_in.value19;
-        	packet1.value20 = packet_in.value20;
-        	packet1.Index = packet_in.Index;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_aq_telemetry_f_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_aq_telemetry_f_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_aq_telemetry_f_pack(system_id, component_id, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 );
-	mavlink_msg_aq_telemetry_f_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 );
-	mavlink_msg_aq_telemetry_f_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_aq_telemetry_f_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_1 , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 );
-	mavlink_msg_aq_telemetry_f_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_autoquad(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_aq_telemetry_f(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // AUTOQUAD_TESTSUITE_H
diff --git a/mavlink/include/mavlink/v1.0/autoquad/version.h b/mavlink/include/mavlink/v1.0/autoquad/version.h
deleted file mode 100644
index d125e92b1..000000000
--- a/mavlink/include/mavlink/v1.0/autoquad/version.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from autoquad.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:27:43 2014"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
- 
-#endif // MAVLINK_VERSION_H
diff --git a/mavlink/include/mavlink/v1.0/checksum.h b/mavlink/include/mavlink/v1.0/checksum.h
deleted file mode 100644
index 948e080a1..000000000
--- a/mavlink/include/mavlink/v1.0/checksum.h
+++ /dev/null
@@ -1,91 +0,0 @@
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef _CHECKSUM_H_
-#define _CHECKSUM_H_
-
-
-/**
- *
- *  CALCULATE THE CHECKSUM
- *
- */
-
-#define X25_INIT_CRC 0xffff
-#define X25_VALIDATE_CRC 0xf0b8
-
-#ifndef HAVE_CRC_ACCUMULATE
-/**
- * @brief Accumulate the X.25 CRC by adding one char at a time.
- *
- * The checksum function adds the hash of one char at a time to the
- * 16 bit checksum (uint16_t).
- *
- * @param data new char to hash
- * @param crcAccum the already accumulated checksum
- **/
-static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
-{
-        /*Accumulate one byte of data into the CRC*/
-        uint8_t tmp;
-
-        tmp = data ^ (uint8_t)(*crcAccum &0xff);
-        tmp ^= (tmp<<4);
-        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
-}
-#endif
-
-/**
- * @brief Initiliaze the buffer for the X.25 CRC
- *
- * @param crcAccum the 16 bit X.25 CRC
- */
-static inline void crc_init(uint16_t* crcAccum)
-{
-        *crcAccum = X25_INIT_CRC;
-}
-
-
-/**
- * @brief Calculates the X.25 checksum on a byte buffer
- *
- * @param  pBuffer buffer containing the byte array to hash
- * @param  length  length of the byte array
- * @return the checksum over the buffer bytes
- **/
-static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
-{
-        uint16_t crcTmp;
-        crc_init(&crcTmp);
-	while (length--) {
-                crc_accumulate(*pBuffer++, &crcTmp);
-        }
-        return crcTmp;
-}
-
-/**
- * @brief Accumulate the X.25 CRC by adding an array of bytes
- *
- * The checksum function adds the hash of one char at a time to the
- * 16 bit checksum (uint16_t).
- *
- * @param data new bytes to hash
- * @param crcAccum the already accumulated checksum
- **/
-static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
-{
-	const uint8_t *p = (const uint8_t *)pBuffer;
-	while (length--) {
-                crc_accumulate(*p++, crcAccum);
-        }
-}
-
-
-
-
-#endif /* _CHECKSUM_H_ */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h
deleted file mode 100644
index 7e1cf765b..000000000
--- a/mavlink/include/mavlink/v1.0/common/common.h
+++ /dev/null
@@ -1,556 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol generated from common.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef COMMON_H
-#define COMMON_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// 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}
-#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}
-#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}}}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_COMMON
-
-// ENUM DEFINITIONS
-
-
-/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */
-#ifndef HAVE_ENUM_MAV_AUTOPILOT
-#define HAVE_ENUM_MAV_AUTOPILOT
-enum MAV_AUTOPILOT
-{
-	MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */
-	MAV_AUTOPILOT_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch | */
-	MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */
-	MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */
-	MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */
-	MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */
-	MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */
-	MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */
-	MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */
-	MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */
-	MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */
-	MAV_AUTOPILOT_FP=11, /* FlexiPilot | */
-	MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://pixhawk.ethz.ch/px4/ | */
-	MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */
-	MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */
-	MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */
-	MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */
-	MAV_AUTOPILOT_ENUM_END=17, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_TYPE
-#define HAVE_ENUM_MAV_TYPE
-enum MAV_TYPE
-{
-	MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */
-	MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */
-	MAV_TYPE_QUADROTOR=2, /* Quadrotor | */
-	MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */
-	MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */
-	MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */
-	MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */
-	MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */
-	MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */
-	MAV_TYPE_ROCKET=9, /* Rocket | */
-	MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */
-	MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */
-	MAV_TYPE_SUBMARINE=12, /* Submarine | */
-	MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */
-	MAV_TYPE_OCTOROTOR=14, /* Octorotor | */
-	MAV_TYPE_TRICOPTER=15, /* Octorotor | */
-	MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */
-	MAV_TYPE_KITE=17, /* Flapping wing | */
-	MAV_TYPE_ENUM_END=18, /*  | */
-};
-#endif
-
-/** @brief These flags encode the MAV mode. */
-#ifndef HAVE_ENUM_MAV_MODE_FLAG
-#define HAVE_ENUM_MAV_MODE_FLAG
-enum MAV_MODE_FLAG
-{
-	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */
-	MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
-	MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
-	MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
-	MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
-	MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
-	MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */
-	MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
-	MAV_MODE_FLAG_ENUM_END=129, /*  | */
-};
-#endif
-
-/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */
-#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION
-#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION
-enum MAV_MODE_FLAG_DECODE_POSITION
-{
-	MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */
-	MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */
-	MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixt bit:   00000100 | */
-	MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit:  00001000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit:  00100000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit:  10000000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /*  | */
-};
-#endif
-
-/** @brief Override command, pauses current mission execution and moves immediately to a position */
-#ifndef HAVE_ENUM_MAV_GOTO
-#define HAVE_ENUM_MAV_GOTO
-enum MAV_GOTO
-{
-	MAV_GOTO_DO_HOLD=0, /* Hold at the current position. | */
-	MAV_GOTO_DO_CONTINUE=1, /* Continue with the next item in mission execution. | */
-	MAV_GOTO_HOLD_AT_CURRENT_POSITION=2, /* Hold at the current position of the system | */
-	MAV_GOTO_HOLD_AT_SPECIFIED_POSITION=3, /* Hold at the position specified in the parameters of the DO_HOLD action | */
-	MAV_GOTO_ENUM_END=4, /*  | */
-};
-#endif
-
-/** @brief These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it
-               simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. */
-#ifndef HAVE_ENUM_MAV_MODE
-#define HAVE_ENUM_MAV_MODE
-enum MAV_MODE
-{
-	MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. No flag is set. | */
-	MAV_MODE_MANUAL_DISARMED=64, /* System is allowed to be active, under manual (RC) control, no stabilization | */
-	MAV_MODE_TEST_DISARMED=66, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */
-	MAV_MODE_STABILIZE_DISARMED=80, /* System is allowed to be active, under assisted RC control. | */
-	MAV_MODE_GUIDED_DISARMED=88, /* System is allowed to be active, under autonomous control, manual setpoint | */
-	MAV_MODE_AUTO_DISARMED=92, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */
-	MAV_MODE_MANUAL_ARMED=192, /* System is allowed to be active, under manual (RC) control, no stabilization | */
-	MAV_MODE_TEST_ARMED=194, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */
-	MAV_MODE_STABILIZE_ARMED=208, /* System is allowed to be active, under assisted RC control. | */
-	MAV_MODE_GUIDED_ARMED=216, /* System is allowed to be active, under autonomous control, manual setpoint | */
-	MAV_MODE_AUTO_ARMED=220, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */
-	MAV_MODE_ENUM_END=221, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_STATE
-#define HAVE_ENUM_MAV_STATE
-enum MAV_STATE
-{
-	MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */
-	MAV_STATE_BOOT=1, /* System is booting up. | */
-	MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */
-	MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */
-	MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */
-	MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */
-	MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */
-	MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */
-	MAV_STATE_ENUM_END=8, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_COMPONENT
-#define HAVE_ENUM_MAV_COMPONENT
-enum MAV_COMPONENT
-{
-	MAV_COMP_ID_ALL=0, /*  | */
-	MAV_COMP_ID_CAMERA=100, /*  | */
-	MAV_COMP_ID_SERVO1=140, /*  | */
-	MAV_COMP_ID_SERVO2=141, /*  | */
-	MAV_COMP_ID_SERVO3=142, /*  | */
-	MAV_COMP_ID_SERVO4=143, /*  | */
-	MAV_COMP_ID_SERVO5=144, /*  | */
-	MAV_COMP_ID_SERVO6=145, /*  | */
-	MAV_COMP_ID_SERVO7=146, /*  | */
-	MAV_COMP_ID_SERVO8=147, /*  | */
-	MAV_COMP_ID_SERVO9=148, /*  | */
-	MAV_COMP_ID_SERVO10=149, /*  | */
-	MAV_COMP_ID_SERVO11=150, /*  | */
-	MAV_COMP_ID_SERVO12=151, /*  | */
-	MAV_COMP_ID_SERVO13=152, /*  | */
-	MAV_COMP_ID_SERVO14=153, /*  | */
-	MAV_COMP_ID_MAPPER=180, /*  | */
-	MAV_COMP_ID_MISSIONPLANNER=190, /*  | */
-	MAV_COMP_ID_PATHPLANNER=195, /*  | */
-	MAV_COMP_ID_IMU=200, /*  | */
-	MAV_COMP_ID_IMU_2=201, /*  | */
-	MAV_COMP_ID_IMU_3=202, /*  | */
-	MAV_COMP_ID_GPS=220, /*  | */
-	MAV_COMP_ID_UDP_BRIDGE=240, /*  | */
-	MAV_COMP_ID_UART_BRIDGE=241, /*  | */
-	MAV_COMP_ID_SYSTEM_CONTROL=250, /*  | */
-	MAV_COMPONENT_ENUM_END=251, /*  | */
-};
-#endif
-
-/** @brief These encode the sensors whose status is sent as part of the SYS_STATUS message. */
-#ifndef HAVE_ENUM_MAV_SYS_STATUS_SENSOR
-#define HAVE_ENUM_MAV_SYS_STATUS_SENSOR
-enum MAV_SYS_STATUS_SENSOR
-{
-	MAV_SYS_STATUS_SENSOR_3D_GYRO=1, /* 0x01 3D gyro | */
-	MAV_SYS_STATUS_SENSOR_3D_ACCEL=2, /* 0x02 3D accelerometer | */
-	MAV_SYS_STATUS_SENSOR_3D_MAG=4, /* 0x04 3D magnetometer | */
-	MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE=8, /* 0x08 absolute pressure | */
-	MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE=16, /* 0x10 differential pressure | */
-	MAV_SYS_STATUS_SENSOR_GPS=32, /* 0x20 GPS | */
-	MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW=64, /* 0x40 optical flow | */
-	MAV_SYS_STATUS_SENSOR_VISION_POSITION=128, /* 0x80 computer vision position | */
-	MAV_SYS_STATUS_SENSOR_LASER_POSITION=256, /* 0x100 laser based position | */
-	MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH=512, /* 0x200 external ground truth (Vicon or Leica) | */
-	MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL=1024, /* 0x400 3D angular rate control | */
-	MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION=2048, /* 0x800 attitude stabilization | */
-	MAV_SYS_STATUS_SENSOR_YAW_POSITION=4096, /* 0x1000 yaw position | */
-	MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL=8192, /* 0x2000 z/altitude control | */
-	MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL=16384, /* 0x4000 x/y position control | */
-	MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS=32768, /* 0x8000 motor outputs / control | */
-	MAV_SYS_STATUS_SENSOR_RC_RECEIVER=65536, /* 0x10000 rc receiver | */
-	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, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_FRAME
-#define HAVE_ENUM_MAV_FRAME
-enum MAV_FRAME
-{
-	MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) | */
-	MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-up (x: north, y: east, z: down). | */
-	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, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE
-#define HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE
-enum MAVLINK_DATA_STREAM_TYPE
-{
-	MAVLINK_DATA_STREAM_IMG_JPEG=1, /*  | */
-	MAVLINK_DATA_STREAM_IMG_BMP=2, /*  | */
-	MAVLINK_DATA_STREAM_IMG_RAW8U=3, /*  | */
-	MAVLINK_DATA_STREAM_IMG_RAW32U=4, /*  | */
-	MAVLINK_DATA_STREAM_IMG_PGM=5, /*  | */
-	MAVLINK_DATA_STREAM_IMG_PNG=6, /*  | */
-	MAVLINK_DATA_STREAM_TYPE_ENUM_END=7, /*  | */
-};
-#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
-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_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 Data stream IDs. A data stream is not a fixed set of messages, but rather a
-     recommendation to the autopilot software. Individual autopilots may or may not obey
-     the recommended messages. */
-#ifndef HAVE_ENUM_MAV_DATA_STREAM
-#define HAVE_ENUM_MAV_DATA_STREAM
-enum MAV_DATA_STREAM
-{
-	MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */
-	MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */
-	MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */
-	MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */
-	MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */
-	MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */
-	MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */
-	MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */
-	MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */
-	MAV_DATA_STREAM_ENUM_END=13, /*  | */
-};
-#endif
-
-/** @brief  The ROI (region of interest) for the vehicle. This can be
-                be used by the vehicle for camera/vehicle attitude alignment (see
-                MAV_CMD_NAV_ROI). */
-#ifndef HAVE_ENUM_MAV_ROI
-#define HAVE_ENUM_MAV_ROI
-enum MAV_ROI
-{
-	MAV_ROI_NONE=0, /* No region of interest. | */
-	MAV_ROI_WPNEXT=1, /* Point toward next MISSION. | */
-	MAV_ROI_WPINDEX=2, /* Point toward given MISSION. | */
-	MAV_ROI_LOCATION=3, /* Point toward fixed location. | */
-	MAV_ROI_TARGET=4, /* Point toward of given id. | */
-	MAV_ROI_ENUM_END=5, /*  | */
-};
-#endif
-
-/** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */
-#ifndef HAVE_ENUM_MAV_CMD_ACK
-#define HAVE_ENUM_MAV_CMD_ACK
-enum MAV_CMD_ACK
-{
-	MAV_CMD_ACK_OK=1, /* Command / mission item is ok. | */
-	MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. | */
-	MAV_CMD_ACK_ERR_ACCESS_DENIED=3, /* The system is refusing to accept this command from this source / communication partner. | */
-	MAV_CMD_ACK_ERR_NOT_SUPPORTED=4, /* Command or mission item is not supported, other commands would be accepted. | */
-	MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=5, /* The coordinate frame of this command / mission item is not supported. | */
-	MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=6, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. | */
-	MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=7, /* The X or latitude value is out of range. | */
-	MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. | */
-	MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. | */
-	MAV_CMD_ACK_ENUM_END=10, /*  | */
-};
-#endif
-
-/** @brief Specifies the datatype of a MAVLink parameter. */
-#ifndef HAVE_ENUM_MAV_PARAM_TYPE
-#define HAVE_ENUM_MAV_PARAM_TYPE
-enum MAV_PARAM_TYPE
-{
-	MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */
-	MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */
-	MAV_PARAM_TYPE_UINT16=3, /* 16-bit unsigned integer | */
-	MAV_PARAM_TYPE_INT16=4, /* 16-bit signed integer | */
-	MAV_PARAM_TYPE_UINT32=5, /* 32-bit unsigned integer | */
-	MAV_PARAM_TYPE_INT32=6, /* 32-bit signed integer | */
-	MAV_PARAM_TYPE_UINT64=7, /* 64-bit unsigned integer | */
-	MAV_PARAM_TYPE_INT64=8, /* 64-bit signed integer | */
-	MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */
-	MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */
-	MAV_PARAM_TYPE_ENUM_END=11, /*  | */
-};
-#endif
-
-/** @brief result from a mavlink command */
-#ifndef HAVE_ENUM_MAV_RESULT
-#define HAVE_ENUM_MAV_RESULT
-enum MAV_RESULT
-{
-	MAV_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */
-	MAV_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */
-	MAV_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */
-	MAV_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */
-	MAV_RESULT_FAILED=4, /* Command executed, but failed | */
-	MAV_RESULT_ENUM_END=5, /*  | */
-};
-#endif
-
-/** @brief result in a mavlink mission ack */
-#ifndef HAVE_ENUM_MAV_MISSION_RESULT
-#define HAVE_ENUM_MAV_MISSION_RESULT
-enum MAV_MISSION_RESULT
-{
-	MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */
-	MAV_MISSION_ERROR=1, /* generic error / not accepting mission commands at all right now | */
-	MAV_MISSION_UNSUPPORTED_FRAME=2, /* coordinate frame is not supported | */
-	MAV_MISSION_UNSUPPORTED=3, /* command is not supported | */
-	MAV_MISSION_NO_SPACE=4, /* mission item exceeds storage space | */
-	MAV_MISSION_INVALID=5, /* one of the parameters has an invalid value | */
-	MAV_MISSION_INVALID_PARAM1=6, /* param1 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM2=7, /* param2 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM3=8, /* param3 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM4=9, /* param4 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM5_X=10, /* x/param5 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM6_Y=11, /* y/param6 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM7=12, /* param7 has an invalid value | */
-	MAV_MISSION_INVALID_SEQUENCE=13, /* received waypoint out of sequence | */
-	MAV_MISSION_DENIED=14, /* not accepting any mission commands from this communication partner | */
-	MAV_MISSION_RESULT_ENUM_END=15, /*  | */
-};
-#endif
-
-/** @brief Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. */
-#ifndef HAVE_ENUM_MAV_SEVERITY
-#define HAVE_ENUM_MAV_SEVERITY
-enum MAV_SEVERITY
-{
-	MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */
-	MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */
-	MAV_SEVERITY_CRITICAL=2, /* Action must be taken immediately. Indicates failure in a primary system. | */
-	MAV_SEVERITY_ERROR=3, /* Indicates an error in secondary/redundant systems. | */
-	MAV_SEVERITY_WARNING=4, /* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */
-	MAV_SEVERITY_NOTICE=5, /* An unusual event has occured, though not an error condition. This should be investigated for the root cause. | */
-	MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */
-	MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */
-	MAV_SEVERITY_ENUM_END=8, /*  | */
-};
-#endif
-
-
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_heartbeat.h"
-#include "./mavlink_msg_sys_status.h"
-#include "./mavlink_msg_system_time.h"
-#include "./mavlink_msg_ping.h"
-#include "./mavlink_msg_change_operator_control.h"
-#include "./mavlink_msg_change_operator_control_ack.h"
-#include "./mavlink_msg_auth_key.h"
-#include "./mavlink_msg_set_mode.h"
-#include "./mavlink_msg_param_request_read.h"
-#include "./mavlink_msg_param_request_list.h"
-#include "./mavlink_msg_param_value.h"
-#include "./mavlink_msg_param_set.h"
-#include "./mavlink_msg_gps_raw_int.h"
-#include "./mavlink_msg_gps_status.h"
-#include "./mavlink_msg_scaled_imu.h"
-#include "./mavlink_msg_raw_imu.h"
-#include "./mavlink_msg_raw_pressure.h"
-#include "./mavlink_msg_scaled_pressure.h"
-#include "./mavlink_msg_attitude.h"
-#include "./mavlink_msg_attitude_quaternion.h"
-#include "./mavlink_msg_local_position_ned.h"
-#include "./mavlink_msg_global_position_int.h"
-#include "./mavlink_msg_rc_channels_scaled.h"
-#include "./mavlink_msg_rc_channels_raw.h"
-#include "./mavlink_msg_servo_output_raw.h"
-#include "./mavlink_msg_mission_request_partial_list.h"
-#include "./mavlink_msg_mission_write_partial_list.h"
-#include "./mavlink_msg_mission_item.h"
-#include "./mavlink_msg_mission_request.h"
-#include "./mavlink_msg_mission_set_current.h"
-#include "./mavlink_msg_mission_current.h"
-#include "./mavlink_msg_mission_request_list.h"
-#include "./mavlink_msg_mission_count.h"
-#include "./mavlink_msg_mission_clear_all.h"
-#include "./mavlink_msg_mission_item_reached.h"
-#include "./mavlink_msg_mission_ack.h"
-#include "./mavlink_msg_set_gps_global_origin.h"
-#include "./mavlink_msg_gps_global_origin.h"
-#include "./mavlink_msg_set_local_position_setpoint.h"
-#include "./mavlink_msg_local_position_setpoint.h"
-#include "./mavlink_msg_global_position_setpoint_int.h"
-#include "./mavlink_msg_set_global_position_setpoint_int.h"
-#include "./mavlink_msg_safety_set_allowed_area.h"
-#include "./mavlink_msg_safety_allowed_area.h"
-#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h"
-#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h"
-#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h"
-#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h"
-#include "./mavlink_msg_set_quad_motors_setpoint.h"
-#include "./mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h"
-#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_request_data_stream.h"
-#include "./mavlink_msg_data_stream.h"
-#include "./mavlink_msg_manual_control.h"
-#include "./mavlink_msg_rc_channels_override.h"
-#include "./mavlink_msg_vfr_hud.h"
-#include "./mavlink_msg_command_long.h"
-#include "./mavlink_msg_command_ack.h"
-#include "./mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h"
-#include "./mavlink_msg_manual_setpoint.h"
-#include "./mavlink_msg_local_position_ned_system_global_offset.h"
-#include "./mavlink_msg_hil_state.h"
-#include "./mavlink_msg_hil_controls.h"
-#include "./mavlink_msg_hil_rc_inputs_raw.h"
-#include "./mavlink_msg_optical_flow.h"
-#include "./mavlink_msg_global_vision_position_estimate.h"
-#include "./mavlink_msg_vision_position_estimate.h"
-#include "./mavlink_msg_vision_speed_estimate.h"
-#include "./mavlink_msg_vicon_position_estimate.h"
-#include "./mavlink_msg_highres_imu.h"
-#include "./mavlink_msg_omnidirectional_flow.h"
-#include "./mavlink_msg_hil_sensor.h"
-#include "./mavlink_msg_sim_state.h"
-#include "./mavlink_msg_radio_status.h"
-#include "./mavlink_msg_file_transfer_start.h"
-#include "./mavlink_msg_file_transfer_dir_list.h"
-#include "./mavlink_msg_file_transfer_res.h"
-#include "./mavlink_msg_hil_gps.h"
-#include "./mavlink_msg_hil_optical_flow.h"
-#include "./mavlink_msg_hil_state_quaternion.h"
-#include "./mavlink_msg_scaled_imu2.h"
-#include "./mavlink_msg_log_request_list.h"
-#include "./mavlink_msg_log_entry.h"
-#include "./mavlink_msg_log_request_data.h"
-#include "./mavlink_msg_log_data.h"
-#include "./mavlink_msg_log_erase.h"
-#include "./mavlink_msg_log_request_end.h"
-#include "./mavlink_msg_gps_inject_data.h"
-#include "./mavlink_msg_gps2_raw.h"
-#include "./mavlink_msg_data_transmission_handshake.h"
-#include "./mavlink_msg_encapsulated_data.h"
-#include "./mavlink_msg_battery_status.h"
-#include "./mavlink_msg_setpoint_8dof.h"
-#include "./mavlink_msg_setpoint_6dof.h"
-#include "./mavlink_msg_memory_vect.h"
-#include "./mavlink_msg_debug_vect.h"
-#include "./mavlink_msg_named_value_float.h"
-#include "./mavlink_msg_named_value_int.h"
-#include "./mavlink_msg_statustext.h"
-#include "./mavlink_msg_debug.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // COMMON_H
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink.h b/mavlink/include/mavlink/v1.0/common/mavlink.h
deleted file mode 100644
index 17b732970..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from common.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "common.h"
-
-#endif // MAVLINK_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
deleted file mode 100644
index 8ddf5bf09..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE ATTITUDE PACKING
-
-#define MAVLINK_MSG_ID_ATTITUDE 30
-
-typedef struct __mavlink_attitude_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float roll; ///< Roll angle (rad, -pi..+pi)
- float pitch; ///< Pitch angle (rad, -pi..+pi)
- float yaw; ///< Yaw angle (rad, -pi..+pi)
- float rollspeed; ///< Roll angular speed (rad/s)
- float pitchspeed; ///< Pitch angular speed (rad/s)
- float yawspeed; ///< Yaw angular speed (rad/s)
-} mavlink_attitude_t;
-
-#define MAVLINK_MSG_ID_ATTITUDE_LEN 28
-#define MAVLINK_MSG_ID_30_LEN 28
-
-#define MAVLINK_MSG_ID_ATTITUDE_CRC 39
-#define MAVLINK_MSG_ID_30_CRC 39
-
-
-
-#define MAVLINK_MESSAGE_INFO_ATTITUDE { \
-	"ATTITUDE", \
-	7, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_t, time_boot_ms) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_t, yaw) }, \
-         { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_t, rollspeed) }, \
-         { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_t, pitchspeed) }, \
-         { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_t, yawspeed) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a attitude 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 roll Roll angle (rad, -pi..+pi)
- * @param pitch Pitch angle (rad, -pi..+pi)
- * @param yaw Yaw angle (rad, -pi..+pi)
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_ATTITUDE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN);
-#else
-	mavlink_attitude_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a attitude 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 roll Roll angle (rad, -pi..+pi)
- * @param pitch Pitch angle (rad, -pi..+pi)
- * @param yaw Yaw angle (rad, -pi..+pi)
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_ATTITUDE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN);
-#else
-	mavlink_attitude_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a attitude 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 attitude C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_t* attitude)
-{
-	return mavlink_msg_attitude_pack(system_id, component_id, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed);
-}
-
-/**
- * @brief Encode a attitude 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 attitude C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_t* attitude)
-{
-	return mavlink_msg_attitude_pack_chan(system_id, component_id, chan, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed);
-}
-
-/**
- * @brief Send a attitude message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param roll Roll angle (rad, -pi..+pi)
- * @param pitch Pitch angle (rad, -pi..+pi)
- * @param yaw Yaw angle (rad, -pi..+pi)
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_attitude_send(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[MAVLINK_MSG_ID_ATTITUDE_LEN];
-	_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;
-	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
-
-// MESSAGE ATTITUDE UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from attitude message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_attitude_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field roll from attitude message
- *
- * @return Roll angle (rad, -pi..+pi)
- */
-static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field pitch from attitude message
- *
- * @return Pitch angle (rad, -pi..+pi)
- */
-static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw from attitude message
- *
- * @return Yaw angle (rad, -pi..+pi)
- */
-static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field rollspeed from attitude message
- *
- * @return Roll angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field pitchspeed from attitude message
- *
- * @return Pitch angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field yawspeed from attitude message
- *
- * @return Yaw angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a attitude message into a struct
- *
- * @param msg The message to decode
- * @param attitude C-struct to decode the message contents into
- */
-static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	attitude->time_boot_ms = mavlink_msg_attitude_get_time_boot_ms(msg);
-	attitude->roll = mavlink_msg_attitude_get_roll(msg);
-	attitude->pitch = mavlink_msg_attitude_get_pitch(msg);
-	attitude->yaw = mavlink_msg_attitude_get_yaw(msg);
-	attitude->rollspeed = mavlink_msg_attitude_get_rollspeed(msg);
-	attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg);
-	attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg);
-#else
-	memcpy(attitude, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_LEN);
-#endif
-}
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
deleted file mode 100644
index 9f8d58759..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h
+++ /dev/null
@@ -1,331 +0,0 @@
-// MESSAGE ATTITUDE_QUATERNION PACKING
-
-#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION 31
-
-typedef struct __mavlink_attitude_quaternion_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float q1; ///< Quaternion component 1
- float q2; ///< Quaternion component 2
- float q3; ///< Quaternion component 3
- float q4; ///< Quaternion component 4
- float rollspeed; ///< Roll angular speed (rad/s)
- float pitchspeed; ///< Pitch angular speed (rad/s)
- float yawspeed; ///< Yaw angular speed (rad/s)
-} mavlink_attitude_quaternion_t;
-
-#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 32
-#define MAVLINK_MSG_ID_31_LEN 32
-
-#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC 246
-#define MAVLINK_MSG_ID_31_CRC 246
-
-
-
-#define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \
-	"ATTITUDE_QUATERNION", \
-	8, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \
-         { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_quaternion_t, q1) }, \
-         { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_quaternion_t, q2) }, \
-         { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_quaternion_t, q3) }, \
-         { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_quaternion_t, q4) }, \
-         { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_t, rollspeed) }, \
-         { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_t, pitchspeed) }, \
-         { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a attitude_quaternion 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 q1 Quaternion component 1
- * @param q2 Quaternion component 2
- * @param q3 Quaternion component 3
- * @param q4 Quaternion component 4
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
-#else
-	mavlink_attitude_quaternion_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
-#endif
-}
-
-/**
- * @brief Pack a attitude_quaternion 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 q1 Quaternion component 1
- * @param q2 Quaternion component 2
- * @param q3 Quaternion component 3
- * @param q4 Quaternion component 4
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
-#else
-	mavlink_attitude_quaternion_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
-#endif
-}
-
-/**
- * @brief Encode a attitude_quaternion 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 attitude_quaternion C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion)
-{
-	return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed);
-}
-
-/**
- * @brief Encode a attitude_quaternion 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 attitude_quaternion C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion)
-{
-	return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed);
-}
-
-/**
- * @brief Send a attitude_quaternion message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param q1 Quaternion component 1
- * @param q2 Quaternion component 2
- * @param q3 Quaternion component 3
- * @param q4 Quaternion component 4
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_attitude_quaternion_send(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[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN];
-	_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;
-	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
-
-// MESSAGE ATTITUDE_QUATERNION UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from attitude_quaternion message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_attitude_quaternion_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field q1 from attitude_quaternion message
- *
- * @return Quaternion component 1
- */
-static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field q2 from attitude_quaternion message
- *
- * @return Quaternion component 2
- */
-static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field q3 from attitude_quaternion message
- *
- * @return Quaternion component 3
- */
-static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field q4 from attitude_quaternion message
- *
- * @return Quaternion component 4
- */
-static inline float mavlink_msg_attitude_quaternion_get_q4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field rollspeed from attitude_quaternion message
- *
- * @return Roll angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_quaternion_get_rollspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field pitchspeed from attitude_quaternion message
- *
- * @return Pitch angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_quaternion_get_pitchspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field yawspeed from attitude_quaternion message
- *
- * @return Yaw angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_quaternion_get_yawspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Decode a attitude_quaternion message into a struct
- *
- * @param msg The message to decode
- * @param attitude_quaternion C-struct to decode the message contents into
- */
-static inline void mavlink_msg_attitude_quaternion_decode(const mavlink_message_t* msg, mavlink_attitude_quaternion_t* attitude_quaternion)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	attitude_quaternion->time_boot_ms = mavlink_msg_attitude_quaternion_get_time_boot_ms(msg);
-	attitude_quaternion->q1 = mavlink_msg_attitude_quaternion_get_q1(msg);
-	attitude_quaternion->q2 = mavlink_msg_attitude_quaternion_get_q2(msg);
-	attitude_quaternion->q3 = mavlink_msg_attitude_quaternion_get_q3(msg);
-	attitude_quaternion->q4 = mavlink_msg_attitude_quaternion_get_q4(msg);
-	attitude_quaternion->rollspeed = mavlink_msg_attitude_quaternion_get_rollspeed(msg);
-	attitude_quaternion->pitchspeed = mavlink_msg_attitude_quaternion_get_pitchspeed(msg);
-	attitude_quaternion->yawspeed = mavlink_msg_attitude_quaternion_get_yawspeed(msg);
-#else
-	memcpy(attitude_quaternion, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
-#endif
-}
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
deleted file mode 100644
index 5703a5987..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h
+++ /dev/null
@@ -1,177 +0,0 @@
-// MESSAGE AUTH_KEY PACKING
-
-#define MAVLINK_MSG_ID_AUTH_KEY 7
-
-typedef struct __mavlink_auth_key_t
-{
- char key[32]; ///< key
-} mavlink_auth_key_t;
-
-#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32
-#define MAVLINK_MSG_ID_7_LEN 32
-
-#define MAVLINK_MSG_ID_AUTH_KEY_CRC 119
-#define MAVLINK_MSG_ID_7_CRC 119
-
-#define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32
-
-#define MAVLINK_MESSAGE_INFO_AUTH_KEY { \
-	"AUTH_KEY", \
-	1, \
-	{  { "key", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a auth_key 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 key key
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const char *key)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN];
-
-	_mav_put_char_array(buf, 0, key, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN);
-#else
-	mavlink_auth_key_t packet;
-
-	mav_array_memcpy(packet.key, key, sizeof(char)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AUTH_KEY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN);
-#endif
-}
-
-/**
- * @brief Pack a auth_key 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 key key
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const char *key)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN];
-
-	_mav_put_char_array(buf, 0, key, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN);
-#else
-	mavlink_auth_key_t packet;
-
-	mav_array_memcpy(packet.key, key, sizeof(char)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AUTH_KEY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN);
-#endif
-}
-
-/**
- * @brief Encode a auth_key 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 auth_key C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key)
-{
-	return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key);
-}
-
-/**
- * @brief Encode a auth_key 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 auth_key C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_auth_key_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key)
-{
-	return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key);
-}
-
-/**
- * @brief Send a auth_key message
- * @param chan MAVLink channel to send the message
- *
- * @param key key
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE AUTH_KEY UNPACKING
-
-
-/**
- * @brief Get field key from auth_key message
- *
- * @return key
- */
-static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key)
-{
-	return _MAV_RETURN_char_array(msg, key, 32,  0);
-}
-
-/**
- * @brief Decode a auth_key message into a struct
- *
- * @param msg The message to decode
- * @param auth_key C-struct to decode the message contents into
- */
-static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_auth_key_get_key(msg, auth_key->key);
-#else
-	memcpy(auth_key, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AUTH_KEY_LEN);
-#endif
-}
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
deleted file mode 100644
index 03e4d569e..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
+++ /dev/null
@@ -1,397 +0,0 @@
-// MESSAGE BATTERY_STATUS PACKING
-
-#define MAVLINK_MSG_ID_BATTERY_STATUS 147
-
-typedef struct __mavlink_battery_status_t
-{
- int32_t current_consumed; ///< Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
- int32_t energy_consumed; ///< Consumed energy, in 100*Joules (intergrated U*I*dt)  (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
- uint16_t voltage_cell_1; ///< Battery voltage of cell 1, in millivolts (1 = 1 millivolt)
- uint16_t voltage_cell_2; ///< Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell
- uint16_t voltage_cell_3; ///< Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell
- uint16_t voltage_cell_4; ///< Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell
- uint16_t voltage_cell_5; ///< Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell
- uint16_t voltage_cell_6; ///< Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell
- int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- uint8_t accu_id; ///< Accupack ID
- int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
-} mavlink_battery_status_t;
-
-#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 24
-#define MAVLINK_MSG_ID_147_LEN 24
-
-#define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 177
-#define MAVLINK_MSG_ID_147_CRC 177
-
-
-
-#define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \
-	"BATTERY_STATUS", \
-	11, \
-	{  { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \
-         { "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \
-         { "voltage_cell_1", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_battery_status_t, voltage_cell_1) }, \
-         { "voltage_cell_2", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_battery_status_t, voltage_cell_2) }, \
-         { "voltage_cell_3", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_battery_status_t, voltage_cell_3) }, \
-         { "voltage_cell_4", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_battery_status_t, voltage_cell_4) }, \
-         { "voltage_cell_5", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_battery_status_t, voltage_cell_5) }, \
-         { "voltage_cell_6", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_battery_status_t, voltage_cell_6) }, \
-         { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_battery_status_t, current_battery) }, \
-         { "accu_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_battery_status_t, accu_id) }, \
-         { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 23, offsetof(mavlink_battery_status_t, battery_remaining) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a battery_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 accu_id Accupack ID
- * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt)
- * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell
- * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
- * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt)  (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
- * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_BATTERY_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
-#else
-	mavlink_battery_status_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a battery_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 accu_id Accupack ID
- * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt)
- * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell
- * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
- * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt)  (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
- * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_BATTERY_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
-#else
-	mavlink_battery_status_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a battery_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 battery_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status)
-{
-	return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining);
-}
-
-/**
- * @brief Encode a battery_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 battery_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status)
-{
-	return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining);
-}
-
-/**
- * @brief Send a battery_status message
- * @param chan MAVLink channel to send the message
- *
- * @param accu_id Accupack ID
- * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt)
- * @param voltage_cell_2 Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_3 Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_4 Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell
- * @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell
- * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- * @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
- * @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt)  (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
- * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_battery_status_send(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[MAVLINK_MSG_ID_BATTERY_STATUS_LEN];
-	_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;
-	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
-
-// MESSAGE BATTERY_STATUS UNPACKING
-
-
-/**
- * @brief Get field accu_id from battery_status message
- *
- * @return Accupack ID
- */
-static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  22);
-}
-
-/**
- * @brief Get field voltage_cell_1 from battery_status message
- *
- * @return Battery voltage of cell 1, in millivolts (1 = 1 millivolt)
- */
-static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field voltage_cell_2 from battery_status message
- *
- * @return Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell
- */
-static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Get field voltage_cell_3 from battery_status message
- *
- * @return Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell
- */
-static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field voltage_cell_4 from battery_status message
- *
- * @return Battery voltage of cell 4, in millivolts (1 = 1 millivolt), -1: no cell
- */
-static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  14);
-}
-
-/**
- * @brief Get field voltage_cell_5 from battery_status message
- *
- * @return Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell
- */
-static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field voltage_cell_6 from battery_status message
- *
- * @return Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell
- */
-static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  18);
-}
-
-/**
- * @brief Get field current_battery from battery_status message
- *
- * @return Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- */
-static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Get field current_consumed from battery_status message
- *
- * @return Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
- */
-static inline int32_t mavlink_msg_battery_status_get_current_consumed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field energy_consumed from battery_status message
- *
- * @return Consumed energy, in 100*Joules (intergrated U*I*dt)  (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
- */
-static inline int32_t mavlink_msg_battery_status_get_energy_consumed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field battery_remaining from battery_status message
- *
- * @return Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
- */
-static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int8_t(msg,  23);
-}
-
-/**
- * @brief Decode a battery_status message into a struct
- *
- * @param msg The message to decode
- * @param battery_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* msg, mavlink_battery_status_t* battery_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	battery_status->current_consumed = mavlink_msg_battery_status_get_current_consumed(msg);
-	battery_status->energy_consumed = mavlink_msg_battery_status_get_energy_consumed(msg);
-	battery_status->voltage_cell_1 = mavlink_msg_battery_status_get_voltage_cell_1(msg);
-	battery_status->voltage_cell_2 = mavlink_msg_battery_status_get_voltage_cell_2(msg);
-	battery_status->voltage_cell_3 = mavlink_msg_battery_status_get_voltage_cell_3(msg);
-	battery_status->voltage_cell_4 = mavlink_msg_battery_status_get_voltage_cell_4(msg);
-	battery_status->voltage_cell_5 = mavlink_msg_battery_status_get_voltage_cell_5(msg);
-	battery_status->voltage_cell_6 = mavlink_msg_battery_status_get_voltage_cell_6(msg);
-	battery_status->current_battery = mavlink_msg_battery_status_get_current_battery(msg);
-	battery_status->accu_id = mavlink_msg_battery_status_get_accu_id(msg);
-	battery_status->battery_remaining = mavlink_msg_battery_status_get_battery_remaining(msg);
-#else
-	memcpy(battery_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
-#endif
-}
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
deleted file mode 100644
index 0b6de930d..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h
+++ /dev/null
@@ -1,237 +0,0 @@
-// MESSAGE CHANGE_OPERATOR_CONTROL PACKING
-
-#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5
-
-typedef struct __mavlink_change_operator_control_t
-{
- uint8_t target_system; ///< System the GCS requests control for
- uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV
- uint8_t version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- char passkey[25]; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
-} mavlink_change_operator_control_t;
-
-#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28
-#define MAVLINK_MSG_ID_5_LEN 28
-
-#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC 217
-#define MAVLINK_MSG_ID_5_CRC 217
-
-#define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25
-
-#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL { \
-	"CHANGE_OPERATOR_CONTROL", \
-	4, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_t, target_system) }, \
-         { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_t, control_request) }, \
-         { "version", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_t, version) }, \
-         { "passkey", NULL, MAVLINK_TYPE_CHAR, 25, 3, offsetof(mavlink_change_operator_control_t, passkey) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a change_operator_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 target_system System the GCS requests control for
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
-#else
-	mavlink_change_operator_control_t packet;
-	packet.target_system = target_system;
-	packet.control_request = control_request;
-	packet.version = version;
-	mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a change_operator_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 target_system System the GCS requests control for
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t control_request,uint8_t version,const char *passkey)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
-#else
-	mavlink_change_operator_control_t packet;
-	packet.target_system = target_system;
-	packet.control_request = control_request;
-	packet.version = version;
-	mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a change_operator_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 change_operator_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control)
-{
-	return mavlink_msg_change_operator_control_pack(system_id, component_id, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey);
-}
-
-/**
- * @brief Encode a change_operator_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 change_operator_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_change_operator_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control)
-{
-	return mavlink_msg_change_operator_control_pack_chan(system_id, component_id, chan, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey);
-}
-
-/**
- * @brief Send a change_operator_control message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System the GCS requests control for
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_change_operator_control_send(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[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN];
-	_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;
-	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
-
-// MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING
-
-
-/**
- * @brief Get field target_system from change_operator_control message
- *
- * @return System the GCS requests control for
- */
-static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field control_request from change_operator_control message
- *
- * @return 0: request control of this MAV, 1: Release control of this MAV
- */
-static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field version from change_operator_control message
- *
- * @return 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- */
-static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field passkey from change_operator_control message
- *
- * @return Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
- */
-static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char *passkey)
-{
-	return _MAV_RETURN_char_array(msg, passkey, 25,  3);
-}
-
-/**
- * @brief Decode a change_operator_control message into a struct
- *
- * @param msg The message to decode
- * @param change_operator_control C-struct to decode the message contents into
- */
-static inline void mavlink_msg_change_operator_control_decode(const mavlink_message_t* msg, mavlink_change_operator_control_t* change_operator_control)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	change_operator_control->target_system = mavlink_msg_change_operator_control_get_target_system(msg);
-	change_operator_control->control_request = mavlink_msg_change_operator_control_get_control_request(msg);
-	change_operator_control->version = mavlink_msg_change_operator_control_get_version(msg);
-	mavlink_msg_change_operator_control_get_passkey(msg, change_operator_control->passkey);
-#else
-	memcpy(change_operator_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
-#endif
-}
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
deleted file mode 100644
index c6f6a28e4..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE CHANGE_OPERATOR_CONTROL_ACK PACKING
-
-#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6
-
-typedef struct __mavlink_change_operator_control_ack_t
-{
- uint8_t gcs_system_id; ///< ID of the GCS this message 
- uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV
- uint8_t ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control
-} mavlink_change_operator_control_ack_t;
-
-#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3
-#define MAVLINK_MSG_ID_6_LEN 3
-
-#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC 104
-#define MAVLINK_MSG_ID_6_CRC 104
-
-
-
-#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK { \
-	"CHANGE_OPERATOR_CONTROL_ACK", \
-	3, \
-	{  { "gcs_system_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_ack_t, gcs_system_id) }, \
-         { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_ack_t, control_request) }, \
-         { "ack", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_ack_t, ack) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a change_operator_control_ack 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 gcs_system_id ID of the GCS this message 
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t gcs_system_id, uint8_t control_request, uint8_t ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN];
-	_mav_put_uint8_t(buf, 0, gcs_system_id);
-	_mav_put_uint8_t(buf, 1, control_request);
-	_mav_put_uint8_t(buf, 2, ack);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
-#else
-	mavlink_change_operator_control_ack_t packet;
-	packet.gcs_system_id = gcs_system_id;
-	packet.control_request = control_request;
-	packet.ack = ack;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Pack a change_operator_control_ack 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 gcs_system_id ID of the GCS this message 
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t gcs_system_id,uint8_t control_request,uint8_t ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN];
-	_mav_put_uint8_t(buf, 0, gcs_system_id);
-	_mav_put_uint8_t(buf, 1, control_request);
-	_mav_put_uint8_t(buf, 2, ack);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
-#else
-	mavlink_change_operator_control_ack_t packet;
-	packet.gcs_system_id = gcs_system_id;
-	packet.control_request = control_request;
-	packet.ack = ack;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Encode a change_operator_control_ack 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 change_operator_control_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack)
-{
-	return mavlink_msg_change_operator_control_ack_pack(system_id, component_id, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack);
-}
-
-/**
- * @brief Encode a change_operator_control_ack 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 change_operator_control_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_change_operator_control_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack)
-{
-	return mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, chan, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack);
-}
-
-/**
- * @brief Send a change_operator_control_ack message
- * @param chan MAVLink channel to send the message
- *
- * @param gcs_system_id ID of the GCS this message 
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_change_operator_control_ack_send(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[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN];
-	_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;
-	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
-
-// MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING
-
-
-/**
- * @brief Get field gcs_system_id from change_operator_control_ack message
- *
- * @return ID of the GCS this message 
- */
-static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field control_request from change_operator_control_ack message
- *
- * @return 0: request control of this MAV, 1: Release control of this MAV
- */
-static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_request(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field ack from change_operator_control_ack message
- *
- * @return 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control
- */
-static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a change_operator_control_ack message into a struct
- *
- * @param msg The message to decode
- * @param change_operator_control_ack C-struct to decode the message contents into
- */
-static inline void mavlink_msg_change_operator_control_ack_decode(const mavlink_message_t* msg, mavlink_change_operator_control_ack_t* change_operator_control_ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	change_operator_control_ack->gcs_system_id = mavlink_msg_change_operator_control_ack_get_gcs_system_id(msg);
-	change_operator_control_ack->control_request = mavlink_msg_change_operator_control_ack_get_control_request(msg);
-	change_operator_control_ack->ack = mavlink_msg_change_operator_control_ack_get_ack(msg);
-#else
-	memcpy(change_operator_control_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
-#endif
-}
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
deleted file mode 100644
index dca2fe681..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE COMMAND_ACK PACKING
-
-#define MAVLINK_MSG_ID_COMMAND_ACK 77
-
-typedef struct __mavlink_command_ack_t
-{
- uint16_t command; ///< Command ID, as defined by MAV_CMD enum.
- uint8_t result; ///< See MAV_RESULT enum
-} mavlink_command_ack_t;
-
-#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 3
-#define MAVLINK_MSG_ID_77_LEN 3
-
-#define MAVLINK_MSG_ID_COMMAND_ACK_CRC 143
-#define MAVLINK_MSG_ID_77_CRC 143
-
-
-
-#define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \
-	"COMMAND_ACK", \
-	2, \
-	{  { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_command_ack_t, command) }, \
-         { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_ack_t, result) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a command_ack 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 command Command ID, as defined by MAV_CMD enum.
- * @param result See MAV_RESULT enum
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t command, uint8_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN];
-	_mav_put_uint16_t(buf, 0, command);
-	_mav_put_uint8_t(buf, 2, result);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
-#else
-	mavlink_command_ack_t packet;
-	packet.command = command;
-	packet.result = result;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Pack a command_ack 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 command Command ID, as defined by MAV_CMD enum.
- * @param result See MAV_RESULT enum
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t command,uint8_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN];
-	_mav_put_uint16_t(buf, 0, command);
-	_mav_put_uint8_t(buf, 2, result);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
-#else
-	mavlink_command_ack_t packet;
-	packet.command = command;
-	packet.result = result;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Encode a command_ack 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 command_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack)
-{
-	return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result);
-}
-
-/**
- * @brief Encode a command_ack 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 command_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack)
-{
-	return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result);
-}
-
-/**
- * @brief Send a command_ack message
- * @param chan MAVLink channel to send the message
- *
- * @param command Command ID, as defined by MAV_CMD enum.
- * @param result See MAV_RESULT enum
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t command, uint8_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN];
-	_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;
-	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
-
-// MESSAGE COMMAND_ACK UNPACKING
-
-
-/**
- * @brief Get field command from command_ack message
- *
- * @return Command ID, as defined by MAV_CMD enum.
- */
-static inline uint16_t mavlink_msg_command_ack_get_command(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field result from command_ack message
- *
- * @return See MAV_RESULT enum
- */
-static inline uint8_t mavlink_msg_command_ack_get_result(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a command_ack message into a struct
- *
- * @param msg The message to decode
- * @param command_ack C-struct to decode the message contents into
- */
-static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, mavlink_command_ack_t* command_ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	command_ack->command = mavlink_msg_command_ack_get_command(msg);
-	command_ack->result = mavlink_msg_command_ack_get_result(msg);
-#else
-	memcpy(command_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMMAND_ACK_LEN);
-#endif
-}
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
deleted file mode 100644
index 8f705c0dd..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h
+++ /dev/null
@@ -1,397 +0,0 @@
-// MESSAGE COMMAND_LONG PACKING
-
-#define MAVLINK_MSG_ID_COMMAND_LONG 76
-
-typedef struct __mavlink_command_long_t
-{
- float param1; ///< Parameter 1, as defined by MAV_CMD enum.
- float param2; ///< Parameter 2, as defined by MAV_CMD enum.
- float param3; ///< Parameter 3, as defined by MAV_CMD enum.
- float param4; ///< Parameter 4, as defined by MAV_CMD enum.
- float param5; ///< Parameter 5, as defined by MAV_CMD enum.
- float param6; ///< Parameter 6, as defined by MAV_CMD enum.
- float param7; ///< Parameter 7, as defined by MAV_CMD enum.
- uint16_t command; ///< Command ID, as defined by MAV_CMD enum.
- uint8_t target_system; ///< System which should execute the command
- uint8_t target_component; ///< Component which should execute the command, 0 for all components
- uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
-} mavlink_command_long_t;
-
-#define MAVLINK_MSG_ID_COMMAND_LONG_LEN 33
-#define MAVLINK_MSG_ID_76_LEN 33
-
-#define MAVLINK_MSG_ID_COMMAND_LONG_CRC 152
-#define MAVLINK_MSG_ID_76_CRC 152
-
-
-
-#define MAVLINK_MESSAGE_INFO_COMMAND_LONG { \
-	"COMMAND_LONG", \
-	11, \
-	{  { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_long_t, param1) }, \
-         { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_long_t, param2) }, \
-         { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_long_t, param3) }, \
-         { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_long_t, param4) }, \
-         { "param5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_command_long_t, param5) }, \
-         { "param6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_command_long_t, param6) }, \
-         { "param7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_command_long_t, param7) }, \
-         { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_command_long_t, command) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_command_long_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_command_long_t, target_component) }, \
-         { "confirmation", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_command_long_t, confirmation) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a command_long 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 target_system System which should execute the command
- * @param target_component Component which should execute the command, 0 for all components
- * @param command Command ID, as defined by MAV_CMD enum.
- * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
- * @param param1 Parameter 1, as defined by MAV_CMD enum.
- * @param param2 Parameter 2, as defined by MAV_CMD enum.
- * @param param3 Parameter 3, as defined by MAV_CMD enum.
- * @param param4 Parameter 4, as defined by MAV_CMD enum.
- * @param param5 Parameter 5, as defined by MAV_CMD enum.
- * @param param6 Parameter 6, as defined by MAV_CMD enum.
- * @param param7 Parameter 7, as defined by MAV_CMD enum.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_COMMAND_LONG_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
-#else
-	mavlink_command_long_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
-#endif
-}
-
-/**
- * @brief Pack a command_long 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 target_system System which should execute the command
- * @param target_component Component which should execute the command, 0 for all components
- * @param command Command ID, as defined by MAV_CMD enum.
- * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
- * @param param1 Parameter 1, as defined by MAV_CMD enum.
- * @param param2 Parameter 2, as defined by MAV_CMD enum.
- * @param param3 Parameter 3, as defined by MAV_CMD enum.
- * @param param4 Parameter 4, as defined by MAV_CMD enum.
- * @param param5 Parameter 5, as defined by MAV_CMD enum.
- * @param param6 Parameter 6, as defined by MAV_CMD enum.
- * @param param7 Parameter 7, as defined by MAV_CMD enum.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_COMMAND_LONG_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
-#else
-	mavlink_command_long_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
-#endif
-}
-
-/**
- * @brief Encode a command_long 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 command_long C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_long_t* command_long)
-{
-	return mavlink_msg_command_long_pack(system_id, component_id, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7);
-}
-
-/**
- * @brief Encode a command_long 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 command_long C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_command_long_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_long_t* command_long)
-{
-	return mavlink_msg_command_long_pack_chan(system_id, component_id, chan, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7);
-}
-
-/**
- * @brief Send a command_long message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System which should execute the command
- * @param target_component Component which should execute the command, 0 for all components
- * @param command Command ID, as defined by MAV_CMD enum.
- * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
- * @param param1 Parameter 1, as defined by MAV_CMD enum.
- * @param param2 Parameter 2, as defined by MAV_CMD enum.
- * @param param3 Parameter 3, as defined by MAV_CMD enum.
- * @param param4 Parameter 4, as defined by MAV_CMD enum.
- * @param param5 Parameter 5, as defined by MAV_CMD enum.
- * @param param6 Parameter 6, as defined by MAV_CMD enum.
- * @param param7 Parameter 7, as defined by MAV_CMD enum.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_command_long_send(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[MAVLINK_MSG_ID_COMMAND_LONG_LEN];
-	_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;
-	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
-
-// MESSAGE COMMAND_LONG UNPACKING
-
-
-/**
- * @brief Get field target_system from command_long message
- *
- * @return System which should execute the command
- */
-static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  30);
-}
-
-/**
- * @brief Get field target_component from command_long message
- *
- * @return Component which should execute the command, 0 for all components
- */
-static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  31);
-}
-
-/**
- * @brief Get field command from command_long message
- *
- * @return Command ID, as defined by MAV_CMD enum.
- */
-static inline uint16_t mavlink_msg_command_long_get_command(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  28);
-}
-
-/**
- * @brief Get field confirmation from command_long message
- *
- * @return 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
- */
-static inline uint8_t mavlink_msg_command_long_get_confirmation(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  32);
-}
-
-/**
- * @brief Get field param1 from command_long message
- *
- * @return Parameter 1, as defined by MAV_CMD enum.
- */
-static inline float mavlink_msg_command_long_get_param1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field param2 from command_long message
- *
- * @return Parameter 2, as defined by MAV_CMD enum.
- */
-static inline float mavlink_msg_command_long_get_param2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field param3 from command_long message
- *
- * @return Parameter 3, as defined by MAV_CMD enum.
- */
-static inline float mavlink_msg_command_long_get_param3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field param4 from command_long message
- *
- * @return Parameter 4, as defined by MAV_CMD enum.
- */
-static inline float mavlink_msg_command_long_get_param4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field param5 from command_long message
- *
- * @return Parameter 5, as defined by MAV_CMD enum.
- */
-static inline float mavlink_msg_command_long_get_param5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field param6 from command_long message
- *
- * @return Parameter 6, as defined by MAV_CMD enum.
- */
-static inline float mavlink_msg_command_long_get_param6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field param7 from command_long message
- *
- * @return Parameter 7, as defined by MAV_CMD enum.
- */
-static inline float mavlink_msg_command_long_get_param7(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a command_long message into a struct
- *
- * @param msg The message to decode
- * @param command_long C-struct to decode the message contents into
- */
-static inline void mavlink_msg_command_long_decode(const mavlink_message_t* msg, mavlink_command_long_t* command_long)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	command_long->param1 = mavlink_msg_command_long_get_param1(msg);
-	command_long->param2 = mavlink_msg_command_long_get_param2(msg);
-	command_long->param3 = mavlink_msg_command_long_get_param3(msg);
-	command_long->param4 = mavlink_msg_command_long_get_param4(msg);
-	command_long->param5 = mavlink_msg_command_long_get_param5(msg);
-	command_long->param6 = mavlink_msg_command_long_get_param6(msg);
-	command_long->param7 = mavlink_msg_command_long_get_param7(msg);
-	command_long->command = mavlink_msg_command_long_get_command(msg);
-	command_long->target_system = mavlink_msg_command_long_get_target_system(msg);
-	command_long->target_component = mavlink_msg_command_long_get_target_component(msg);
-	command_long->confirmation = mavlink_msg_command_long_get_confirmation(msg);
-#else
-	memcpy(command_long, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMMAND_LONG_LEN);
-#endif
-}
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
deleted file mode 100644
index dc0768e12..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE DATA_STREAM PACKING
-
-#define MAVLINK_MSG_ID_DATA_STREAM 67
-
-typedef struct __mavlink_data_stream_t
-{
- uint16_t message_rate; ///< The requested interval between two messages of this type
- uint8_t stream_id; ///< The ID of the requested data stream
- uint8_t on_off; ///< 1 stream is enabled, 0 stream is stopped.
-} mavlink_data_stream_t;
-
-#define MAVLINK_MSG_ID_DATA_STREAM_LEN 4
-#define MAVLINK_MSG_ID_67_LEN 4
-
-#define MAVLINK_MSG_ID_DATA_STREAM_CRC 21
-#define MAVLINK_MSG_ID_67_CRC 21
-
-
-
-#define MAVLINK_MESSAGE_INFO_DATA_STREAM { \
-	"DATA_STREAM", \
-	3, \
-	{  { "message_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_data_stream_t, message_rate) }, \
-         { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_data_stream_t, stream_id) }, \
-         { "on_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_data_stream_t, on_off) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data_stream 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 stream_id The ID of the requested data stream
- * @param message_rate The requested interval between two messages of this type
- * @param on_off 1 stream is enabled, 0 stream is stopped.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t stream_id, uint16_t message_rate, uint8_t on_off)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN];
-	_mav_put_uint16_t(buf, 0, message_rate);
-	_mav_put_uint8_t(buf, 2, stream_id);
-	_mav_put_uint8_t(buf, 3, on_off);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_STREAM_LEN);
-#else
-	mavlink_data_stream_t packet;
-	packet.message_rate = message_rate;
-	packet.stream_id = stream_id;
-	packet.on_off = on_off;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA_STREAM;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_STREAM_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data_stream 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 stream_id The ID of the requested data stream
- * @param message_rate The requested interval between two messages of this type
- * @param on_off 1 stream is enabled, 0 stream is stopped.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t stream_id,uint16_t message_rate,uint8_t on_off)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN];
-	_mav_put_uint16_t(buf, 0, message_rate);
-	_mav_put_uint8_t(buf, 2, stream_id);
-	_mav_put_uint8_t(buf, 3, on_off);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_STREAM_LEN);
-#else
-	mavlink_data_stream_t packet;
-	packet.message_rate = message_rate;
-	packet.stream_id = stream_id;
-	packet.on_off = on_off;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA_STREAM;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_STREAM_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data_stream 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 data_stream C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream)
-{
-	return mavlink_msg_data_stream_pack(system_id, component_id, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off);
-}
-
-/**
- * @brief Encode a data_stream 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 data_stream C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream)
-{
-	return mavlink_msg_data_stream_pack_chan(system_id, component_id, chan, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off);
-}
-
-/**
- * @brief Send a data_stream message
- * @param chan MAVLink channel to send the message
- *
- * @param stream_id The ID of the requested data stream
- * @param message_rate The requested interval between two messages of this type
- * @param on_off 1 stream is enabled, 0 stream is stopped.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data_stream_send(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[MAVLINK_MSG_ID_DATA_STREAM_LEN];
-	_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;
-	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
-
-// MESSAGE DATA_STREAM UNPACKING
-
-
-/**
- * @brief Get field stream_id from data_stream message
- *
- * @return The ID of the requested data stream
- */
-static inline uint8_t mavlink_msg_data_stream_get_stream_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field message_rate from data_stream message
- *
- * @return The requested interval between two messages of this type
- */
-static inline uint16_t mavlink_msg_data_stream_get_message_rate(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field on_off from data_stream message
- *
- * @return 1 stream is enabled, 0 stream is stopped.
- */
-static inline uint8_t mavlink_msg_data_stream_get_on_off(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Decode a data_stream message into a struct
- *
- * @param msg The message to decode
- * @param data_stream C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data_stream_decode(const mavlink_message_t* msg, mavlink_data_stream_t* data_stream)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data_stream->message_rate = mavlink_msg_data_stream_get_message_rate(msg);
-	data_stream->stream_id = mavlink_msg_data_stream_get_stream_id(msg);
-	data_stream->on_off = mavlink_msg_data_stream_get_on_off(msg);
-#else
-	memcpy(data_stream, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA_STREAM_LEN);
-#endif
-}
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
deleted file mode 100644
index fdd8fddd8..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING
-
-#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 130
-
-typedef struct __mavlink_data_transmission_handshake_t
-{
- uint32_t size; ///< total data size in bytes (set on ACK only)
- uint16_t width; ///< Width of a matrix or image
- uint16_t height; ///< Height of a matrix or image
- uint16_t packets; ///< number of packets beeing sent (set on ACK only)
- uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)
- uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)
- uint8_t jpg_quality; ///< JPEG quality out of [1,100]
-} mavlink_data_transmission_handshake_t;
-
-#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 13
-#define MAVLINK_MSG_ID_130_LEN 13
-
-#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC 29
-#define MAVLINK_MSG_ID_130_CRC 29
-
-
-
-#define MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE { \
-	"DATA_TRANSMISSION_HANDSHAKE", \
-	7, \
-	{  { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_data_transmission_handshake_t, size) }, \
-         { "width", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_data_transmission_handshake_t, width) }, \
-         { "height", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_data_transmission_handshake_t, height) }, \
-         { "packets", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_data_transmission_handshake_t, packets) }, \
-         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_data_transmission_handshake_t, type) }, \
-         { "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_data_transmission_handshake_t, payload) }, \
-         { "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data_transmission_handshake 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 type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)
- * @param size total data size in bytes (set on ACK only)
- * @param width Width of a matrix or image
- * @param height Height of a matrix or image
- * @param packets number of packets beeing sent (set on ACK only)
- * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)
- * @param jpg_quality JPEG quality out of [1,100]
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
-#else
-	mavlink_data_transmission_handshake_t packet;
-	packet.size = size;
-	packet.width = width;
-	packet.height = height;
-	packet.packets = packets;
-	packet.type = type;
-	packet.payload = payload;
-	packet.jpg_quality = jpg_quality;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data_transmission_handshake 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 type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)
- * @param size total data size in bytes (set on ACK only)
- * @param width Width of a matrix or image
- * @param height Height of a matrix or image
- * @param packets number of packets beeing sent (set on ACK only)
- * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)
- * @param jpg_quality JPEG quality out of [1,100]
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
-#else
-	mavlink_data_transmission_handshake_t packet;
-	packet.size = size;
-	packet.width = width;
-	packet.height = height;
-	packet.packets = packets;
-	packet.type = type;
-	packet.payload = payload;
-	packet.jpg_quality = jpg_quality;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data_transmission_handshake 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 data_transmission_handshake C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake)
-{
-	return mavlink_msg_data_transmission_handshake_pack(system_id, component_id, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality);
-}
-
-/**
- * @brief Encode a data_transmission_handshake 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 data_transmission_handshake C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake)
-{
-	return mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, chan, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality);
-}
-
-/**
- * @brief Send a data_transmission_handshake message
- * @param chan MAVLink channel to send the message
- *
- * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)
- * @param size total data size in bytes (set on ACK only)
- * @param width Width of a matrix or image
- * @param height Height of a matrix or image
- * @param packets number of packets beeing sent (set on ACK only)
- * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)
- * @param jpg_quality JPEG quality out of [1,100]
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data_transmission_handshake_send(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[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
-	_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;
-	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
-
-// MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING
-
-
-/**
- * @brief Get field type from data_transmission_handshake message
- *
- * @return type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)
- */
-static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field size from data_transmission_handshake message
- *
- * @return total data size in bytes (set on ACK only)
- */
-static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field width from data_transmission_handshake message
- *
- * @return Width of a matrix or image
- */
-static inline uint16_t mavlink_msg_data_transmission_handshake_get_width(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field height from data_transmission_handshake message
- *
- * @return Height of a matrix or image
- */
-static inline uint16_t mavlink_msg_data_transmission_handshake_get_height(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field packets from data_transmission_handshake message
- *
- * @return number of packets beeing sent (set on ACK only)
- */
-static inline uint16_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field payload from data_transmission_handshake message
- *
- * @return payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)
- */
-static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field jpg_quality from data_transmission_handshake message
- *
- * @return JPEG quality out of [1,100]
- */
-static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Decode a data_transmission_handshake message into a struct
- *
- * @param msg The message to decode
- * @param data_transmission_handshake C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_message_t* msg, mavlink_data_transmission_handshake_t* data_transmission_handshake)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg);
-	data_transmission_handshake->width = mavlink_msg_data_transmission_handshake_get_width(msg);
-	data_transmission_handshake->height = mavlink_msg_data_transmission_handshake_get_height(msg);
-	data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg);
-	data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg);
-	data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg);
-	data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg);
-#else
-	memcpy(data_transmission_handshake, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h
deleted file mode 100644
index 9a6ed87ee..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE DEBUG PACKING
-
-#define MAVLINK_MSG_ID_DEBUG 254
-
-typedef struct __mavlink_debug_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float value; ///< DEBUG value
- uint8_t ind; ///< index of debug variable
-} mavlink_debug_t;
-
-#define MAVLINK_MSG_ID_DEBUG_LEN 9
-#define MAVLINK_MSG_ID_254_LEN 9
-
-#define MAVLINK_MSG_ID_DEBUG_CRC 46
-#define MAVLINK_MSG_ID_254_CRC 46
-
-
-
-#define MAVLINK_MESSAGE_INFO_DEBUG { \
-	"DEBUG", \
-	3, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_debug_t, time_boot_ms) }, \
-         { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_debug_t, value) }, \
-         { "ind", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_debug_t, ind) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a debug 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 ind index of debug variable
- * @param value DEBUG value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint32_t time_boot_ms, uint8_t ind, float value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DEBUG_LEN];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, value);
-	_mav_put_uint8_t(buf, 8, ind);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN);
-#else
-	mavlink_debug_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.value = value;
-	packet.ind = ind;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DEBUG;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN);
-#endif
-}
-
-/**
- * @brief Pack a debug 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 ind index of debug variable
- * @param value DEBUG value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint32_t time_boot_ms,uint8_t ind,float value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DEBUG_LEN];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, value);
-	_mav_put_uint8_t(buf, 8, ind);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN);
-#else
-	mavlink_debug_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.value = value;
-	packet.ind = ind;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DEBUG;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN);
-#endif
-}
-
-/**
- * @brief Encode a debug 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 debug C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_t* debug)
-{
-	return mavlink_msg_debug_pack(system_id, component_id, msg, debug->time_boot_ms, debug->ind, debug->value);
-}
-
-/**
- * @brief Encode a debug 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 debug C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_debug_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_t* debug)
-{
-	return mavlink_msg_debug_pack_chan(system_id, component_id, chan, msg, debug->time_boot_ms, debug->ind, debug->value);
-}
-
-/**
- * @brief Send a debug message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param ind index of debug variable
- * @param value DEBUG value
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DEBUG_LEN];
-	_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;
-	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
-
-// MESSAGE DEBUG UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from debug message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_debug_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field ind from debug message
- *
- * @return index of debug variable
- */
-static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field value from debug message
- *
- * @return DEBUG value
- */
-static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Decode a debug message into a struct
- *
- * @param msg The message to decode
- * @param debug C-struct to decode the message contents into
- */
-static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlink_debug_t* debug)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	debug->time_boot_ms = mavlink_msg_debug_get_time_boot_ms(msg);
-	debug->value = mavlink_msg_debug_get_value(msg);
-	debug->ind = mavlink_msg_debug_get_ind(msg);
-#else
-	memcpy(debug, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DEBUG_LEN);
-#endif
-}
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
deleted file mode 100644
index 6cfc75212..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h
+++ /dev/null
@@ -1,259 +0,0 @@
-// MESSAGE DEBUG_VECT PACKING
-
-#define MAVLINK_MSG_ID_DEBUG_VECT 250
-
-typedef struct __mavlink_debug_vect_t
-{
- uint64_t time_usec; ///< Timestamp
- float x; ///< x
- float y; ///< y
- float z; ///< z
- char name[10]; ///< Name
-} mavlink_debug_vect_t;
-
-#define MAVLINK_MSG_ID_DEBUG_VECT_LEN 30
-#define MAVLINK_MSG_ID_250_LEN 30
-
-#define MAVLINK_MSG_ID_DEBUG_VECT_CRC 49
-#define MAVLINK_MSG_ID_250_CRC 49
-
-#define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10
-
-#define MAVLINK_MESSAGE_INFO_DEBUG_VECT { \
-	"DEBUG_VECT", \
-	5, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_debug_vect_t, time_usec) }, \
-         { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_debug_vect_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_debug_vect_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_debug_vect_t, z) }, \
-         { "name", NULL, MAVLINK_TYPE_CHAR, 10, 20, offsetof(mavlink_debug_vect_t, name) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a debug_vect 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 name Name
- * @param time_usec Timestamp
- * @param x x
- * @param y y
- * @param z z
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const char *name, uint64_t time_usec, float x, float y, float z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
-#else
-	mavlink_debug_vect_t packet;
-	packet.time_usec = time_usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a debug_vect 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 name Name
- * @param time_usec Timestamp
- * @param x x
- * @param y y
- * @param z z
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const char *name,uint64_t time_usec,float x,float y,float z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
-#else
-	mavlink_debug_vect_t packet;
-	packet.time_usec = time_usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a debug_vect 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 debug_vect C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect)
-{
-	return mavlink_msg_debug_vect_pack(system_id, component_id, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z);
-}
-
-/**
- * @brief Encode a debug_vect 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 debug_vect C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_debug_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect)
-{
-	return mavlink_msg_debug_vect_pack_chan(system_id, component_id, chan, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z);
-}
-
-/**
- * @brief Send a debug_vect message
- * @param chan MAVLink channel to send the message
- *
- * @param name Name
- * @param time_usec Timestamp
- * @param x x
- * @param y y
- * @param z z
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_debug_vect_send(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[MAVLINK_MSG_ID_DEBUG_VECT_LEN];
-	_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;
-	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
-
-// MESSAGE DEBUG_VECT UNPACKING
-
-
-/**
- * @brief Get field name from debug_vect message
- *
- * @return Name
- */
-static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char *name)
-{
-	return _MAV_RETURN_char_array(msg, name, 10,  20);
-}
-
-/**
- * @brief Get field time_usec from debug_vect message
- *
- * @return Timestamp
- */
-static inline uint64_t mavlink_msg_debug_vect_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field x from debug_vect message
- *
- * @return x
- */
-static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field y from debug_vect message
- *
- * @return y
- */
-static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field z from debug_vect message
- *
- * @return z
- */
-static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a debug_vect message into a struct
- *
- * @param msg The message to decode
- * @param debug_vect C-struct to decode the message contents into
- */
-static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, mavlink_debug_vect_t* debug_vect)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	debug_vect->time_usec = mavlink_msg_debug_vect_get_time_usec(msg);
-	debug_vect->x = mavlink_msg_debug_vect_get_x(msg);
-	debug_vect->y = mavlink_msg_debug_vect_get_y(msg);
-	debug_vect->z = mavlink_msg_debug_vect_get_z(msg);
-	mavlink_msg_debug_vect_get_name(msg, debug_vect->name);
-#else
-	memcpy(debug_vect, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DEBUG_VECT_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
deleted file mode 100644
index 5900ea838..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h
+++ /dev/null
@@ -1,193 +0,0 @@
-// MESSAGE ENCAPSULATED_DATA PACKING
-
-#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 131
-
-typedef struct __mavlink_encapsulated_data_t
-{
- uint16_t seqnr; ///< sequence number (starting with 0 on every transmission)
- uint8_t data[253]; ///< image data bytes
-} mavlink_encapsulated_data_t;
-
-#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255
-#define MAVLINK_MSG_ID_131_LEN 255
-
-#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC 223
-#define MAVLINK_MSG_ID_131_CRC 223
-
-#define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253
-
-#define MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA { \
-	"ENCAPSULATED_DATA", \
-	2, \
-	{  { "seqnr", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_encapsulated_data_t, seqnr) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 253, 2, offsetof(mavlink_encapsulated_data_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a encapsulated_data 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 seqnr sequence number (starting with 0 on every transmission)
- * @param data image data bytes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t seqnr, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN];
-	_mav_put_uint16_t(buf, 0, seqnr);
-	_mav_put_uint8_t_array(buf, 2, data, 253);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
-#else
-	mavlink_encapsulated_data_t packet;
-	packet.seqnr = seqnr;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
-#endif
-}
-
-/**
- * @brief Pack a encapsulated_data 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 seqnr sequence number (starting with 0 on every transmission)
- * @param data image data bytes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t seqnr,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN];
-	_mav_put_uint16_t(buf, 0, seqnr);
-	_mav_put_uint8_t_array(buf, 2, data, 253);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
-#else
-	mavlink_encapsulated_data_t packet;
-	packet.seqnr = seqnr;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
-#endif
-}
-
-/**
- * @brief Encode a encapsulated_data 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 encapsulated_data C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data)
-{
-	return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data);
-}
-
-/**
- * @brief Encode a encapsulated_data 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 encapsulated_data C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_encapsulated_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data)
-{
-	return mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, chan, msg, encapsulated_data->seqnr, encapsulated_data->data);
-}
-
-/**
- * @brief Send a encapsulated_data message
- * @param chan MAVLink channel to send the message
- *
- * @param seqnr sequence number (starting with 0 on every transmission)
- * @param data image data bytes
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN];
-	_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;
-	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
-
-// MESSAGE ENCAPSULATED_DATA UNPACKING
-
-
-/**
- * @brief Get field seqnr from encapsulated_data message
- *
- * @return sequence number (starting with 0 on every transmission)
- */
-static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field data from encapsulated_data message
- *
- * @return image data bytes
- */
-static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 253,  2);
-}
-
-/**
- * @brief Decode a encapsulated_data message into a struct
- *
- * @param msg The message to decode
- * @param encapsulated_data C-struct to decode the message contents into
- */
-static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg);
-	mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data);
-#else
-	memcpy(encapsulated_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
-#endif
-}
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
deleted file mode 100644
index 4f31698d5..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h
+++ /dev/null
@@ -1,215 +0,0 @@
-// MESSAGE FILE_TRANSFER_DIR_LIST PACKING
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST 111
-
-typedef struct __mavlink_file_transfer_dir_list_t
-{
- uint64_t transfer_uid; ///< Unique transfer ID
- char dir_path[240]; ///< Directory path to list
- uint8_t flags; ///< RESERVED
-} mavlink_file_transfer_dir_list_t;
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN 249
-#define MAVLINK_MSG_ID_111_LEN 249
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC 93
-#define MAVLINK_MSG_ID_111_CRC 93
-
-#define MAVLINK_MSG_FILE_TRANSFER_DIR_LIST_FIELD_DIR_PATH_LEN 240
-
-#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST { \
-	"FILE_TRANSFER_DIR_LIST", \
-	3, \
-	{  { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_dir_list_t, transfer_uid) }, \
-         { "dir_path", NULL, MAVLINK_TYPE_CHAR, 240, 8, offsetof(mavlink_file_transfer_dir_list_t, dir_path) }, \
-         { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 248, offsetof(mavlink_file_transfer_dir_list_t, flags) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a file_transfer_dir_list 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 transfer_uid Unique transfer ID
- * @param dir_path Directory path to list
- * @param flags RESERVED
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_file_transfer_dir_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint64_t transfer_uid, const char *dir_path, uint8_t flags)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN];
-	_mav_put_uint64_t(buf, 0, transfer_uid);
-	_mav_put_uint8_t(buf, 248, flags);
-	_mav_put_char_array(buf, 8, dir_path, 240);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
-#else
-	mavlink_file_transfer_dir_list_t packet;
-	packet.transfer_uid = transfer_uid;
-	packet.flags = flags;
-	mav_array_memcpy(packet.dir_path, dir_path, sizeof(char)*240);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a file_transfer_dir_list 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 transfer_uid Unique transfer ID
- * @param dir_path Directory path to list
- * @param flags RESERVED
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_file_transfer_dir_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint64_t transfer_uid,const char *dir_path,uint8_t flags)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN];
-	_mav_put_uint64_t(buf, 0, transfer_uid);
-	_mav_put_uint8_t(buf, 248, flags);
-	_mav_put_char_array(buf, 8, dir_path, 240);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
-#else
-	mavlink_file_transfer_dir_list_t packet;
-	packet.transfer_uid = transfer_uid;
-	packet.flags = flags;
-	mav_array_memcpy(packet.dir_path, dir_path, sizeof(char)*240);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a file_transfer_dir_list 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 file_transfer_dir_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_file_transfer_dir_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list)
-{
-	return mavlink_msg_file_transfer_dir_list_pack(system_id, component_id, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags);
-}
-
-/**
- * @brief Encode a file_transfer_dir_list 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 file_transfer_dir_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_file_transfer_dir_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list)
-{
-	return mavlink_msg_file_transfer_dir_list_pack_chan(system_id, component_id, chan, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags);
-}
-
-/**
- * @brief Send a file_transfer_dir_list message
- * @param chan MAVLink channel to send the message
- *
- * @param transfer_uid Unique transfer ID
- * @param dir_path Directory path to list
- * @param flags RESERVED
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_file_transfer_dir_list_send(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[MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN];
-	_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;
-	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
-
-// MESSAGE FILE_TRANSFER_DIR_LIST UNPACKING
-
-
-/**
- * @brief Get field transfer_uid from file_transfer_dir_list message
- *
- * @return Unique transfer ID
- */
-static inline uint64_t mavlink_msg_file_transfer_dir_list_get_transfer_uid(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field dir_path from file_transfer_dir_list message
- *
- * @return Directory path to list
- */
-static inline uint16_t mavlink_msg_file_transfer_dir_list_get_dir_path(const mavlink_message_t* msg, char *dir_path)
-{
-	return _MAV_RETURN_char_array(msg, dir_path, 240,  8);
-}
-
-/**
- * @brief Get field flags from file_transfer_dir_list message
- *
- * @return RESERVED
- */
-static inline uint8_t mavlink_msg_file_transfer_dir_list_get_flags(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  248);
-}
-
-/**
- * @brief Decode a file_transfer_dir_list message into a struct
- *
- * @param msg The message to decode
- * @param file_transfer_dir_list C-struct to decode the message contents into
- */
-static inline void mavlink_msg_file_transfer_dir_list_decode(const mavlink_message_t* msg, mavlink_file_transfer_dir_list_t* file_transfer_dir_list)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	file_transfer_dir_list->transfer_uid = mavlink_msg_file_transfer_dir_list_get_transfer_uid(msg);
-	mavlink_msg_file_transfer_dir_list_get_dir_path(msg, file_transfer_dir_list->dir_path);
-	file_transfer_dir_list->flags = mavlink_msg_file_transfer_dir_list_get_flags(msg);
-#else
-	memcpy(file_transfer_dir_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
-#endif
-}
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
deleted file mode 100644
index fc6247fac..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE FILE_TRANSFER_RES PACKING
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_RES 112
-
-typedef struct __mavlink_file_transfer_res_t
-{
- uint64_t transfer_uid; ///< Unique transfer ID
- uint8_t result; ///< 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device
-} mavlink_file_transfer_res_t;
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN 9
-#define MAVLINK_MSG_ID_112_LEN 9
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC 124
-#define MAVLINK_MSG_ID_112_CRC 124
-
-
-
-#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES { \
-	"FILE_TRANSFER_RES", \
-	2, \
-	{  { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_res_t, transfer_uid) }, \
-         { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_file_transfer_res_t, result) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a file_transfer_res 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 transfer_uid Unique transfer ID
- * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_file_transfer_res_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint64_t transfer_uid, uint8_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN];
-	_mav_put_uint64_t(buf, 0, transfer_uid);
-	_mav_put_uint8_t(buf, 8, result);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
-#else
-	mavlink_file_transfer_res_t packet;
-	packet.transfer_uid = transfer_uid;
-	packet.result = result;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_RES;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
-#endif
-}
-
-/**
- * @brief Pack a file_transfer_res 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 transfer_uid Unique transfer ID
- * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_file_transfer_res_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint64_t transfer_uid,uint8_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN];
-	_mav_put_uint64_t(buf, 0, transfer_uid);
-	_mav_put_uint8_t(buf, 8, result);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
-#else
-	mavlink_file_transfer_res_t packet;
-	packet.transfer_uid = transfer_uid;
-	packet.result = result;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_RES;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
-#endif
-}
-
-/**
- * @brief Encode a file_transfer_res 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 file_transfer_res C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_file_transfer_res_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res)
-{
-	return mavlink_msg_file_transfer_res_pack(system_id, component_id, msg, file_transfer_res->transfer_uid, file_transfer_res->result);
-}
-
-/**
- * @brief Encode a file_transfer_res 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 file_transfer_res C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_file_transfer_res_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res)
-{
-	return mavlink_msg_file_transfer_res_pack_chan(system_id, component_id, chan, msg, file_transfer_res->transfer_uid, file_transfer_res->result);
-}
-
-/**
- * @brief Send a file_transfer_res message
- * @param chan MAVLink channel to send the message
- *
- * @param transfer_uid Unique transfer ID
- * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_file_transfer_res_send(mavlink_channel_t chan, uint64_t transfer_uid, uint8_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN];
-	_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;
-	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
-
-// MESSAGE FILE_TRANSFER_RES UNPACKING
-
-
-/**
- * @brief Get field transfer_uid from file_transfer_res message
- *
- * @return Unique transfer ID
- */
-static inline uint64_t mavlink_msg_file_transfer_res_get_transfer_uid(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field result from file_transfer_res message
- *
- * @return 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device
- */
-static inline uint8_t mavlink_msg_file_transfer_res_get_result(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Decode a file_transfer_res message into a struct
- *
- * @param msg The message to decode
- * @param file_transfer_res C-struct to decode the message contents into
- */
-static inline void mavlink_msg_file_transfer_res_decode(const mavlink_message_t* msg, mavlink_file_transfer_res_t* file_transfer_res)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	file_transfer_res->transfer_uid = mavlink_msg_file_transfer_res_get_transfer_uid(msg);
-	file_transfer_res->result = mavlink_msg_file_transfer_res_get_result(msg);
-#else
-	memcpy(file_transfer_res, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
-#endif
-}
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
deleted file mode 100644
index 05be77339..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h
+++ /dev/null
@@ -1,259 +0,0 @@
-// MESSAGE FILE_TRANSFER_START PACKING
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_START 110
-
-typedef struct __mavlink_file_transfer_start_t
-{
- uint64_t transfer_uid; ///< Unique transfer ID
- uint32_t file_size; ///< File size in bytes
- char dest_path[240]; ///< Destination path
- uint8_t direction; ///< Transfer direction: 0: from requester, 1: to requester
- uint8_t flags; ///< RESERVED
-} mavlink_file_transfer_start_t;
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN 254
-#define MAVLINK_MSG_ID_110_LEN 254
-
-#define MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC 235
-#define MAVLINK_MSG_ID_110_CRC 235
-
-#define MAVLINK_MSG_FILE_TRANSFER_START_FIELD_DEST_PATH_LEN 240
-
-#define MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START { \
-	"FILE_TRANSFER_START", \
-	5, \
-	{  { "transfer_uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_file_transfer_start_t, transfer_uid) }, \
-         { "file_size", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_file_transfer_start_t, file_size) }, \
-         { "dest_path", NULL, MAVLINK_TYPE_CHAR, 240, 12, offsetof(mavlink_file_transfer_start_t, dest_path) }, \
-         { "direction", NULL, MAVLINK_TYPE_UINT8_T, 0, 252, offsetof(mavlink_file_transfer_start_t, direction) }, \
-         { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 253, offsetof(mavlink_file_transfer_start_t, flags) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a file_transfer_start 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 transfer_uid Unique transfer ID
- * @param dest_path Destination path
- * @param direction Transfer direction: 0: from requester, 1: to requester
- * @param file_size File size in bytes
- * @param flags RESERVED
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_file_transfer_start_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
-#else
-	mavlink_file_transfer_start_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_START;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
-#endif
-}
-
-/**
- * @brief Pack a file_transfer_start 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 transfer_uid Unique transfer ID
- * @param dest_path Destination path
- * @param direction Transfer direction: 0: from requester, 1: to requester
- * @param file_size File size in bytes
- * @param flags RESERVED
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_file_transfer_start_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
-#else
-	mavlink_file_transfer_start_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_START;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
-#endif
-}
-
-/**
- * @brief Encode a file_transfer_start 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 file_transfer_start C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_file_transfer_start_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start)
-{
-	return mavlink_msg_file_transfer_start_pack(system_id, component_id, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags);
-}
-
-/**
- * @brief Encode a file_transfer_start 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 file_transfer_start C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_file_transfer_start_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start)
-{
-	return mavlink_msg_file_transfer_start_pack_chan(system_id, component_id, chan, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags);
-}
-
-/**
- * @brief Send a file_transfer_start message
- * @param chan MAVLink channel to send the message
- *
- * @param transfer_uid Unique transfer ID
- * @param dest_path Destination path
- * @param direction Transfer direction: 0: from requester, 1: to requester
- * @param file_size File size in bytes
- * @param flags RESERVED
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_file_transfer_start_send(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[MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN];
-	_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;
-	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
-
-// MESSAGE FILE_TRANSFER_START UNPACKING
-
-
-/**
- * @brief Get field transfer_uid from file_transfer_start message
- *
- * @return Unique transfer ID
- */
-static inline uint64_t mavlink_msg_file_transfer_start_get_transfer_uid(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field dest_path from file_transfer_start message
- *
- * @return Destination path
- */
-static inline uint16_t mavlink_msg_file_transfer_start_get_dest_path(const mavlink_message_t* msg, char *dest_path)
-{
-	return _MAV_RETURN_char_array(msg, dest_path, 240,  12);
-}
-
-/**
- * @brief Get field direction from file_transfer_start message
- *
- * @return Transfer direction: 0: from requester, 1: to requester
- */
-static inline uint8_t mavlink_msg_file_transfer_start_get_direction(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  252);
-}
-
-/**
- * @brief Get field file_size from file_transfer_start message
- *
- * @return File size in bytes
- */
-static inline uint32_t mavlink_msg_file_transfer_start_get_file_size(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  8);
-}
-
-/**
- * @brief Get field flags from file_transfer_start message
- *
- * @return RESERVED
- */
-static inline uint8_t mavlink_msg_file_transfer_start_get_flags(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  253);
-}
-
-/**
- * @brief Decode a file_transfer_start message into a struct
- *
- * @param msg The message to decode
- * @param file_transfer_start C-struct to decode the message contents into
- */
-static inline void mavlink_msg_file_transfer_start_decode(const mavlink_message_t* msg, mavlink_file_transfer_start_t* file_transfer_start)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	file_transfer_start->transfer_uid = mavlink_msg_file_transfer_start_get_transfer_uid(msg);
-	file_transfer_start->file_size = mavlink_msg_file_transfer_start_get_file_size(msg);
-	mavlink_msg_file_transfer_start_get_dest_path(msg, file_transfer_start->dest_path);
-	file_transfer_start->direction = mavlink_msg_file_transfer_start_get_direction(msg);
-	file_transfer_start->flags = mavlink_msg_file_transfer_start_get_flags(msg);
-#else
-	memcpy(file_transfer_start, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
-#endif
-}
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
deleted file mode 100644
index 7ed3d2a63..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h
+++ /dev/null
@@ -1,353 +0,0 @@
-// MESSAGE GLOBAL_POSITION_INT PACKING
-
-#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 33
-
-typedef struct __mavlink_global_position_int_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- int32_t lat; ///< Latitude, expressed as * 1E7
- int32_t lon; ///< Longitude, expressed as * 1E7
- int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL
- int32_t relative_alt; ///< Altitude above ground in meters, expressed as * 1000 (millimeters)
- int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100
- int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100
- int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100
- uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
-} mavlink_global_position_int_t;
-
-#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 28
-#define MAVLINK_MSG_ID_33_LEN 28
-
-#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC 104
-#define MAVLINK_MSG_ID_33_CRC 104
-
-
-
-#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT { \
-	"GLOBAL_POSITION_INT", \
-	9, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_global_position_int_t, time_boot_ms) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_int_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_int_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_global_position_int_t, alt) }, \
-         { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_global_position_int_t, relative_alt) }, \
-         { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_global_position_int_t, vx) }, \
-         { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_global_position_int_t, vy) }, \
-         { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_global_position_int_t, vz) }, \
-         { "hdg", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_global_position_int_t, hdg) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a global_position_int 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 lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL
- * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
-#else
-	mavlink_global_position_int_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a global_position_int 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 lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL
- * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
-#else
-	mavlink_global_position_int_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a global_position_int 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 global_position_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int)
-{
-	return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg);
-}
-
-/**
- * @brief Encode a global_position_int 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 global_position_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int)
-{
-	return mavlink_msg_global_position_int_pack_chan(system_id, component_id, chan, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg);
-}
-
-/**
- * @brief Send a global_position_int message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL
- * @param relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_global_position_int_send(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[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN];
-	_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;
-	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
-
-// MESSAGE GLOBAL_POSITION_INT UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from global_position_int message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_global_position_int_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field lat from global_position_int message
- *
- * @return Latitude, expressed as * 1E7
- */
-static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field lon from global_position_int message
- *
- * @return Longitude, expressed as * 1E7
- */
-static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field alt from global_position_int message
- *
- * @return Altitude in meters, expressed as * 1000 (millimeters), above MSL
- */
-static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  12);
-}
-
-/**
- * @brief Get field relative_alt from global_position_int message
- *
- * @return Altitude above ground in meters, expressed as * 1000 (millimeters)
- */
-static inline int32_t mavlink_msg_global_position_int_get_relative_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  16);
-}
-
-/**
- * @brief Get field vx from global_position_int message
- *
- * @return Ground X Speed (Latitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Get field vy from global_position_int message
- *
- * @return Ground Y Speed (Longitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  22);
-}
-
-/**
- * @brief Get field vz from global_position_int message
- *
- * @return Ground Z Speed (Altitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  24);
-}
-
-/**
- * @brief Get field hdg from global_position_int message
- *
- * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  26);
-}
-
-/**
- * @brief Decode a global_position_int message into a struct
- *
- * @param msg The message to decode
- * @param global_position_int C-struct to decode the message contents into
- */
-static inline void mavlink_msg_global_position_int_decode(const mavlink_message_t* msg, mavlink_global_position_int_t* global_position_int)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	global_position_int->time_boot_ms = mavlink_msg_global_position_int_get_time_boot_ms(msg);
-	global_position_int->lat = mavlink_msg_global_position_int_get_lat(msg);
-	global_position_int->lon = mavlink_msg_global_position_int_get_lon(msg);
-	global_position_int->alt = mavlink_msg_global_position_int_get_alt(msg);
-	global_position_int->relative_alt = mavlink_msg_global_position_int_get_relative_alt(msg);
-	global_position_int->vx = mavlink_msg_global_position_int_get_vx(msg);
-	global_position_int->vy = mavlink_msg_global_position_int_get_vy(msg);
-	global_position_int->vz = mavlink_msg_global_position_int_get_vz(msg);
-	global_position_int->hdg = mavlink_msg_global_position_int_get_hdg(msg);
-#else
-	memcpy(global_position_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
-#endif
-}
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
deleted file mode 100644
index 1a1c97199..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE GLOBAL_POSITION_SETPOINT_INT PACKING
-
-#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT 52
-
-typedef struct __mavlink_global_position_setpoint_int_t
-{
- int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7
- int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7
- int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up)
- int16_t yaw; ///< Desired yaw angle in degrees * 100
- uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
-} mavlink_global_position_setpoint_int_t;
-
-#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN 15
-#define MAVLINK_MSG_ID_52_LEN 15
-
-#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC 141
-#define MAVLINK_MSG_ID_52_CRC 141
-
-
-
-#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT { \
-	"GLOBAL_POSITION_SETPOINT_INT", \
-	5, \
-	{  { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_global_position_setpoint_int_t, latitude) }, \
-         { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_setpoint_int_t, longitude) }, \
-         { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_setpoint_int_t, altitude) }, \
-         { "yaw", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_global_position_setpoint_int_t, yaw) }, \
-         { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_global_position_setpoint_int_t, coordinate_frame) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a global_position_setpoint_int 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 coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @param yaw Desired yaw angle in degrees * 100
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#else
-	mavlink_global_position_setpoint_int_t packet;
-	packet.latitude = latitude;
-	packet.longitude = longitude;
-	packet.altitude = altitude;
-	packet.yaw = yaw;
-	packet.coordinate_frame = coordinate_frame;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a global_position_setpoint_int 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 coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @param yaw Desired yaw angle in degrees * 100
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#else
-	mavlink_global_position_setpoint_int_t packet;
-	packet.latitude = latitude;
-	packet.longitude = longitude;
-	packet.altitude = altitude;
-	packet.yaw = yaw;
-	packet.coordinate_frame = coordinate_frame;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a global_position_setpoint_int 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 global_position_setpoint_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int)
-{
-	return mavlink_msg_global_position_setpoint_int_pack(system_id, component_id, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw);
-}
-
-/**
- * @brief Encode a global_position_setpoint_int 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 global_position_setpoint_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int)
-{
-	return mavlink_msg_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw);
-}
-
-/**
- * @brief Send a global_position_setpoint_int message
- * @param chan MAVLink channel to send the message
- *
- * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @param yaw Desired yaw angle in degrees * 100
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_global_position_setpoint_int_send(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[MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN];
-	_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;
-	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
-
-// MESSAGE GLOBAL_POSITION_SETPOINT_INT UNPACKING
-
-
-/**
- * @brief Get field coordinate_frame from global_position_setpoint_int message
- *
- * @return Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
- */
-static inline uint8_t mavlink_msg_global_position_setpoint_int_get_coordinate_frame(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Get field latitude from global_position_setpoint_int message
- *
- * @return Latitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field longitude from global_position_setpoint_int message
- *
- * @return Longitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field altitude from global_position_setpoint_int message
- *
- * @return Altitude (WGS84), in meters * 1000 (positive for up)
- */
-static inline int32_t mavlink_msg_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field yaw from global_position_setpoint_int message
- *
- * @return Desired yaw angle in degrees * 100
- */
-static inline int16_t mavlink_msg_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Decode a global_position_setpoint_int message into a struct
- *
- * @param msg The message to decode
- * @param global_position_setpoint_int C-struct to decode the message contents into
- */
-static inline void mavlink_msg_global_position_setpoint_int_decode(const mavlink_message_t* msg, mavlink_global_position_setpoint_int_t* global_position_setpoint_int)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	global_position_setpoint_int->latitude = mavlink_msg_global_position_setpoint_int_get_latitude(msg);
-	global_position_setpoint_int->longitude = mavlink_msg_global_position_setpoint_int_get_longitude(msg);
-	global_position_setpoint_int->altitude = mavlink_msg_global_position_setpoint_int_get_altitude(msg);
-	global_position_setpoint_int->yaw = mavlink_msg_global_position_setpoint_int_get_yaw(msg);
-	global_position_setpoint_int->coordinate_frame = mavlink_msg_global_position_setpoint_int_get_coordinate_frame(msg);
-#else
-	memcpy(global_position_setpoint_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-}
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
deleted file mode 100644
index f7be74c91..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE GLOBAL_VISION_POSITION_ESTIMATE PACKING
-
-#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE 101
-
-typedef struct __mavlink_global_vision_position_estimate_t
-{
- uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
- float x; ///< Global X position
- float y; ///< Global Y position
- float z; ///< Global Z position
- float roll; ///< Roll angle in rad
- float pitch; ///< Pitch angle in rad
- float yaw; ///< Yaw angle in rad
-} mavlink_global_vision_position_estimate_t;
-
-#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN 32
-#define MAVLINK_MSG_ID_101_LEN 32
-
-#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC 102
-#define MAVLINK_MSG_ID_101_CRC 102
-
-
-
-#define MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE { \
-	"GLOBAL_VISION_POSITION_ESTIMATE", \
-	7, \
-	{  { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_vision_position_estimate_t, usec) }, \
-         { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_vision_position_estimate_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_vision_position_estimate_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_vision_position_estimate_t, z) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_vision_position_estimate_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_vision_position_estimate_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_vision_position_estimate_t, yaw) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a global_vision_position_estimate 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
-#else
-	mavlink_global_vision_position_estimate_t packet;
-	packet.usec = usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a global_vision_position_estimate 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
-#else
-	mavlink_global_vision_position_estimate_t packet;
-	packet.usec = usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a global_vision_position_estimate 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 global_vision_position_estimate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate)
-{
-	return mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw);
-}
-
-/**
- * @brief Encode a global_vision_position_estimate 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 global_vision_position_estimate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate)
-{
-	return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw);
-}
-
-/**
- * @brief Send a global_vision_position_estimate message
- * @param chan MAVLink channel to send the message
- *
- * @param usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_global_vision_position_estimate_send(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[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN];
-	_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;
-	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
-
-// MESSAGE GLOBAL_VISION_POSITION_ESTIMATE UNPACKING
-
-
-/**
- * @brief Get field usec from global_vision_position_estimate message
- *
- * @return Timestamp (microseconds, synced to UNIX time or since system boot)
- */
-static inline uint64_t mavlink_msg_global_vision_position_estimate_get_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field x from global_vision_position_estimate message
- *
- * @return Global X position
- */
-static inline float mavlink_msg_global_vision_position_estimate_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field y from global_vision_position_estimate message
- *
- * @return Global Y position
- */
-static inline float mavlink_msg_global_vision_position_estimate_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field z from global_vision_position_estimate message
- *
- * @return Global Z position
- */
-static inline float mavlink_msg_global_vision_position_estimate_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field roll from global_vision_position_estimate message
- *
- * @return Roll angle in rad
- */
-static inline float mavlink_msg_global_vision_position_estimate_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field pitch from global_vision_position_estimate message
- *
- * @return Pitch angle in rad
- */
-static inline float mavlink_msg_global_vision_position_estimate_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field yaw from global_vision_position_estimate message
- *
- * @return Yaw angle in rad
- */
-static inline float mavlink_msg_global_vision_position_estimate_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Decode a global_vision_position_estimate message into a struct
- *
- * @param msg The message to decode
- * @param global_vision_position_estimate C-struct to decode the message contents into
- */
-static inline void mavlink_msg_global_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_global_vision_position_estimate_t* global_vision_position_estimate)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	global_vision_position_estimate->usec = mavlink_msg_global_vision_position_estimate_get_usec(msg);
-	global_vision_position_estimate->x = mavlink_msg_global_vision_position_estimate_get_x(msg);
-	global_vision_position_estimate->y = mavlink_msg_global_vision_position_estimate_get_y(msg);
-	global_vision_position_estimate->z = mavlink_msg_global_vision_position_estimate_get_z(msg);
-	global_vision_position_estimate->roll = mavlink_msg_global_vision_position_estimate_get_roll(msg);
-	global_vision_position_estimate->pitch = mavlink_msg_global_vision_position_estimate_get_pitch(msg);
-	global_vision_position_estimate->yaw = mavlink_msg_global_vision_position_estimate_get_yaw(msg);
-#else
-	memcpy(global_vision_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
-#endif
-}
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
deleted file mode 100644
index 17e5bd002..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h
+++ /dev/null
@@ -1,419 +0,0 @@
-// MESSAGE GPS2_RAW PACKING
-
-#define MAVLINK_MSG_ID_GPS2_RAW 124
-
-typedef struct __mavlink_gps2_raw_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- int32_t lat; ///< Latitude (WGS84), in degrees * 1E7
- int32_t lon; ///< Longitude (WGS84), in degrees * 1E7
- int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up)
- uint32_t dgps_age; ///< Age of DGPS info
- uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- 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.
- uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255
- uint8_t dgps_numch; ///< Number of DGPS satellites
-} mavlink_gps2_raw_t;
-
-#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35
-#define MAVLINK_MSG_ID_124_LEN 35
-
-#define MAVLINK_MSG_ID_GPS2_RAW_CRC 87
-#define MAVLINK_MSG_ID_124_CRC 87
-
-
-
-#define MAVLINK_MESSAGE_INFO_GPS2_RAW { \
-	"GPS2_RAW", \
-	12, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_raw_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_raw_t, alt) }, \
-         { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \
-         { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps2_raw_t, eph) }, \
-         { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps2_raw_t, epv) }, \
-         { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_raw_t, vel) }, \
-         { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_gps2_raw_t, cog) }, \
-         { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \
-         { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \
-         { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a gps2_raw 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- * @param dgps_numch Number of DGPS satellites
- * @param dgps_age Age of DGPS info
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_GPS2_RAW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
-#else
-	mavlink_gps2_raw_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS2_RAW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN);
-#endif
-}
-
-/**
- * @brief Pack a gps2_raw 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- * @param dgps_numch Number of DGPS satellites
- * @param dgps_age Age of DGPS info
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_GPS2_RAW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
-#else
-	mavlink_gps2_raw_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS2_RAW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN);
-#endif
-}
-
-/**
- * @brief Encode a gps2_raw 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 gps2_raw C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw)
-{
-	return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age);
-}
-
-/**
- * @brief Encode a gps2_raw 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 gps2_raw C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw)
-{
-	return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age);
-}
-
-/**
- * @brief Send a gps2_raw message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- * @param dgps_numch Number of DGPS satellites
- * @param dgps_age Age of DGPS info
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_gps2_raw_send(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[MAVLINK_MSG_ID_GPS2_RAW_LEN];
-	_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;
-	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
-
-// MESSAGE GPS2_RAW UNPACKING
-
-
-/**
- * @brief Get field time_usec from gps2_raw message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field fix_type from gps2_raw message
- *
- * @return 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.
- */
-static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  32);
-}
-
-/**
- * @brief Get field lat from gps2_raw message
- *
- * @return Latitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_gps2_raw_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field lon from gps2_raw message
- *
- * @return Longitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_gps2_raw_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  12);
-}
-
-/**
- * @brief Get field alt from gps2_raw message
- *
- * @return Altitude (WGS84), in meters * 1000 (positive for up)
- */
-static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  16);
-}
-
-/**
- * @brief Get field eph from gps2_raw message
- *
- * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  24);
-}
-
-/**
- * @brief Get field epv from gps2_raw message
- *
- * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  26);
-}
-
-/**
- * @brief Get field vel from gps2_raw message
- *
- * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_gps2_raw_get_vel(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  28);
-}
-
-/**
- * @brief Get field cog from gps2_raw message
- *
- * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_gps2_raw_get_cog(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  30);
-}
-
-/**
- * @brief Get field satellites_visible from gps2_raw message
- *
- * @return Number of satellites visible. If unknown, set to 255
- */
-static inline uint8_t mavlink_msg_gps2_raw_get_satellites_visible(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  33);
-}
-
-/**
- * @brief Get field dgps_numch from gps2_raw message
- *
- * @return Number of DGPS satellites
- */
-static inline uint8_t mavlink_msg_gps2_raw_get_dgps_numch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  34);
-}
-
-/**
- * @brief Get field dgps_age from gps2_raw message
- *
- * @return Age of DGPS info
- */
-static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  20);
-}
-
-/**
- * @brief Decode a gps2_raw message into a struct
- *
- * @param msg The message to decode
- * @param gps2_raw C-struct to decode the message contents into
- */
-static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mavlink_gps2_raw_t* gps2_raw)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	gps2_raw->time_usec = mavlink_msg_gps2_raw_get_time_usec(msg);
-	gps2_raw->lat = mavlink_msg_gps2_raw_get_lat(msg);
-	gps2_raw->lon = mavlink_msg_gps2_raw_get_lon(msg);
-	gps2_raw->alt = mavlink_msg_gps2_raw_get_alt(msg);
-	gps2_raw->dgps_age = mavlink_msg_gps2_raw_get_dgps_age(msg);
-	gps2_raw->eph = mavlink_msg_gps2_raw_get_eph(msg);
-	gps2_raw->epv = mavlink_msg_gps2_raw_get_epv(msg);
-	gps2_raw->vel = mavlink_msg_gps2_raw_get_vel(msg);
-	gps2_raw->cog = mavlink_msg_gps2_raw_get_cog(msg);
-	gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg);
-	gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg);
-	gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg);
-#else
-	memcpy(gps2_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RAW_LEN);
-#endif
-}
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
deleted file mode 100644
index 016e9cb0e..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE GPS_GLOBAL_ORIGIN PACKING
-
-#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN 49
-
-typedef struct __mavlink_gps_global_origin_t
-{
- int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7
- int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7
- int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up)
-} mavlink_gps_global_origin_t;
-
-#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN 12
-#define MAVLINK_MSG_ID_49_LEN 12
-
-#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC 39
-#define MAVLINK_MSG_ID_49_CRC 39
-
-
-
-#define MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN { \
-	"GPS_GLOBAL_ORIGIN", \
-	3, \
-	{  { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_global_origin_t, latitude) }, \
-         { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_global_origin_t, longitude) }, \
-         { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_global_origin_t, altitude) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a gps_global_origin 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 latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       int32_t latitude, int32_t longitude, int32_t altitude)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN];
-	_mav_put_int32_t(buf, 0, latitude);
-	_mav_put_int32_t(buf, 4, longitude);
-	_mav_put_int32_t(buf, 8, altitude);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
-#else
-	mavlink_gps_global_origin_t packet;
-	packet.latitude = latitude;
-	packet.longitude = longitude;
-	packet.altitude = altitude;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-}
-
-/**
- * @brief Pack a gps_global_origin 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 latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           int32_t latitude,int32_t longitude,int32_t altitude)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN];
-	_mav_put_int32_t(buf, 0, latitude);
-	_mav_put_int32_t(buf, 4, longitude);
-	_mav_put_int32_t(buf, 8, altitude);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
-#else
-	mavlink_gps_global_origin_t packet;
-	packet.latitude = latitude;
-	packet.longitude = longitude;
-	packet.altitude = altitude;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-}
-
-/**
- * @brief Encode a gps_global_origin 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 gps_global_origin C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin)
-{
-	return mavlink_msg_gps_global_origin_pack(system_id, component_id, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude);
-}
-
-/**
- * @brief Encode a gps_global_origin 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 gps_global_origin C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin)
-{
-	return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude);
-}
-
-/**
- * @brief Send a gps_global_origin message
- * @param chan MAVLink channel to send the message
- *
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN];
-	_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;
-	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
-
-// MESSAGE GPS_GLOBAL_ORIGIN UNPACKING
-
-
-/**
- * @brief Get field latitude from gps_global_origin message
- *
- * @return Latitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_gps_global_origin_get_latitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field longitude from gps_global_origin message
- *
- * @return Longitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_gps_global_origin_get_longitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field altitude from gps_global_origin message
- *
- * @return Altitude (WGS84), in meters * 1000 (positive for up)
- */
-static inline int32_t mavlink_msg_gps_global_origin_get_altitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Decode a gps_global_origin message into a struct
- *
- * @param msg The message to decode
- * @param gps_global_origin C-struct to decode the message contents into
- */
-static inline void mavlink_msg_gps_global_origin_decode(const mavlink_message_t* msg, mavlink_gps_global_origin_t* gps_global_origin)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	gps_global_origin->latitude = mavlink_msg_gps_global_origin_get_latitude(msg);
-	gps_global_origin->longitude = mavlink_msg_gps_global_origin_get_longitude(msg);
-	gps_global_origin->altitude = mavlink_msg_gps_global_origin_get_altitude(msg);
-#else
-	memcpy(gps_global_origin, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-}
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
deleted file mode 100644
index 485d8a4af..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h
+++ /dev/null
@@ -1,237 +0,0 @@
-// MESSAGE GPS_INJECT_DATA PACKING
-
-#define MAVLINK_MSG_ID_GPS_INJECT_DATA 123
-
-typedef struct __mavlink_gps_inject_data_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t len; ///< data length
- uint8_t data[110]; ///< raw data (110 is enough for 12 satellites of RTCMv2)
-} mavlink_gps_inject_data_t;
-
-#define MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN 113
-#define MAVLINK_MSG_ID_123_LEN 113
-
-#define MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC 250
-#define MAVLINK_MSG_ID_123_CRC 250
-
-#define MAVLINK_MSG_GPS_INJECT_DATA_FIELD_DATA_LEN 110
-
-#define MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA { \
-	"GPS_INJECT_DATA", \
-	4, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_inject_data_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_inject_data_t, target_component) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_inject_data_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 110, 3, offsetof(mavlink_gps_inject_data_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a gps_inject_data 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 target_system System ID
- * @param target_component Component ID
- * @param len data length
- * @param data raw data (110 is enough for 12 satellites of RTCMv2)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
-#else
-	mavlink_gps_inject_data_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
-#endif
-}
-
-/**
- * @brief Pack a gps_inject_data 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 target_system System ID
- * @param target_component Component ID
- * @param len data length
- * @param data raw data (110 is enough for 12 satellites of RTCMv2)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps_inject_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
-#else
-	mavlink_gps_inject_data_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
-#endif
-}
-
-/**
- * @brief Encode a gps_inject_data 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 gps_inject_data C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps_inject_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data)
-{
-	return mavlink_msg_gps_inject_data_pack(system_id, component_id, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data);
-}
-
-/**
- * @brief Encode a gps_inject_data 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 gps_inject_data C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps_inject_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data)
-{
-	return mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, chan, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data);
-}
-
-/**
- * @brief Send a gps_inject_data message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param len data length
- * @param data raw data (110 is enough for 12 satellites of RTCMv2)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_gps_inject_data_send(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[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
-	_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;
-	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
-
-// MESSAGE GPS_INJECT_DATA UNPACKING
-
-
-/**
- * @brief Get field target_system from gps_inject_data message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_gps_inject_data_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from gps_inject_data message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_gps_inject_data_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field len from gps_inject_data message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_gps_inject_data_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field data from gps_inject_data message
- *
- * @return raw data (110 is enough for 12 satellites of RTCMv2)
- */
-static inline uint16_t mavlink_msg_gps_inject_data_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 110,  3);
-}
-
-/**
- * @brief Decode a gps_inject_data message into a struct
- *
- * @param msg The message to decode
- * @param gps_inject_data C-struct to decode the message contents into
- */
-static inline void mavlink_msg_gps_inject_data_decode(const mavlink_message_t* msg, mavlink_gps_inject_data_t* gps_inject_data)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	gps_inject_data->target_system = mavlink_msg_gps_inject_data_get_target_system(msg);
-	gps_inject_data->target_component = mavlink_msg_gps_inject_data_get_target_component(msg);
-	gps_inject_data->len = mavlink_msg_gps_inject_data_get_len(msg);
-	mavlink_msg_gps_inject_data_get_data(msg, gps_inject_data->data);
-#else
-	memcpy(gps_inject_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
-#endif
-}
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
deleted file mode 100644
index a105f8cda..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE GPS_RAW_INT PACKING
-
-#define MAVLINK_MSG_ID_GPS_RAW_INT 24
-
-typedef struct __mavlink_gps_raw_int_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- int32_t lat; ///< Latitude (WGS84), in degrees * 1E7
- int32_t lon; ///< Longitude (WGS84), in degrees * 1E7
- int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up)
- uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- 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.
- uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255
-} mavlink_gps_raw_int_t;
-
-#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30
-#define MAVLINK_MSG_ID_24_LEN 30
-
-#define MAVLINK_MSG_ID_GPS_RAW_INT_CRC 24
-#define MAVLINK_MSG_ID_24_CRC 24
-
-
-
-#define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \
-	"GPS_RAW_INT", \
-	10, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, time_usec) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_raw_int_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_raw_int_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_raw_int_t, alt) }, \
-         { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_gps_raw_int_t, eph) }, \
-         { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_gps_raw_int_t, epv) }, \
-         { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps_raw_int_t, vel) }, \
-         { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps_raw_int_t, cog) }, \
-         { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_gps_raw_int_t, fix_type) }, \
-         { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_gps_raw_int_t, satellites_visible) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a gps_raw_int 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_GPS_RAW_INT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
-#else
-	mavlink_gps_raw_int_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a gps_raw_int 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_GPS_RAW_INT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
-#else
-	mavlink_gps_raw_int_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a gps_raw_int 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 gps_raw_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int)
-{
-	return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible);
-}
-
-/**
- * @brief Encode a gps_raw_int 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 gps_raw_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int)
-{
-	return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible);
-}
-
-/**
- * @brief Send a gps_raw_int message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_gps_raw_int_send(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[MAVLINK_MSG_ID_GPS_RAW_INT_LEN];
-	_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;
-	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
-
-// MESSAGE GPS_RAW_INT UNPACKING
-
-
-/**
- * @brief Get field time_usec from gps_raw_int message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_gps_raw_int_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field fix_type from gps_raw_int message
- *
- * @return 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.
- */
-static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  28);
-}
-
-/**
- * @brief Get field lat from gps_raw_int message
- *
- * @return Latitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field lon from gps_raw_int message
- *
- * @return Longitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  12);
-}
-
-/**
- * @brief Get field alt from gps_raw_int message
- *
- * @return Altitude (WGS84), in meters * 1000 (positive for up)
- */
-static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  16);
-}
-
-/**
- * @brief Get field eph from gps_raw_int message
- *
- * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  20);
-}
-
-/**
- * @brief Get field epv from gps_raw_int message
- *
- * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  22);
-}
-
-/**
- * @brief Get field vel from gps_raw_int message
- *
- * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  24);
-}
-
-/**
- * @brief Get field cog from gps_raw_int message
- *
- * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
- */
-static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  26);
-}
-
-/**
- * @brief Get field satellites_visible from gps_raw_int message
- *
- * @return Number of satellites visible. If unknown, set to 255
- */
-static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  29);
-}
-
-/**
- * @brief Decode a gps_raw_int message into a struct
- *
- * @param msg The message to decode
- * @param gps_raw_int C-struct to decode the message contents into
- */
-static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, mavlink_gps_raw_int_t* gps_raw_int)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	gps_raw_int->time_usec = mavlink_msg_gps_raw_int_get_time_usec(msg);
-	gps_raw_int->lat = mavlink_msg_gps_raw_int_get_lat(msg);
-	gps_raw_int->lon = mavlink_msg_gps_raw_int_get_lon(msg);
-	gps_raw_int->alt = mavlink_msg_gps_raw_int_get_alt(msg);
-	gps_raw_int->eph = mavlink_msg_gps_raw_int_get_eph(msg);
-	gps_raw_int->epv = mavlink_msg_gps_raw_int_get_epv(msg);
-	gps_raw_int->vel = mavlink_msg_gps_raw_int_get_vel(msg);
-	gps_raw_int->cog = mavlink_msg_gps_raw_int_get_cog(msg);
-	gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg);
-	gps_raw_int->satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg);
-#else
-	memcpy(gps_raw_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
-#endif
-}
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
deleted file mode 100644
index 28d6b57d1..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h
+++ /dev/null
@@ -1,285 +0,0 @@
-// MESSAGE GPS_STATUS PACKING
-
-#define MAVLINK_MSG_ID_GPS_STATUS 25
-
-typedef struct __mavlink_gps_status_t
-{
- uint8_t satellites_visible; ///< Number of satellites visible
- uint8_t satellite_prn[20]; ///< Global satellite ID
- uint8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization
- uint8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite
- uint8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg.
- uint8_t satellite_snr[20]; ///< Signal to noise ratio of satellite
-} mavlink_gps_status_t;
-
-#define MAVLINK_MSG_ID_GPS_STATUS_LEN 101
-#define MAVLINK_MSG_ID_25_LEN 101
-
-#define MAVLINK_MSG_ID_GPS_STATUS_CRC 23
-#define MAVLINK_MSG_ID_25_CRC 23
-
-#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_PRN_LEN 20
-#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_USED_LEN 20
-#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_ELEVATION_LEN 20
-#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_AZIMUTH_LEN 20
-#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_SNR_LEN 20
-
-#define MAVLINK_MESSAGE_INFO_GPS_STATUS { \
-	"GPS_STATUS", \
-	6, \
-	{  { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_status_t, satellites_visible) }, \
-         { "satellite_prn", NULL, MAVLINK_TYPE_UINT8_T, 20, 1, offsetof(mavlink_gps_status_t, satellite_prn) }, \
-         { "satellite_used", NULL, MAVLINK_TYPE_UINT8_T, 20, 21, offsetof(mavlink_gps_status_t, satellite_used) }, \
-         { "satellite_elevation", NULL, MAVLINK_TYPE_UINT8_T, 20, 41, offsetof(mavlink_gps_status_t, satellite_elevation) }, \
-         { "satellite_azimuth", NULL, MAVLINK_TYPE_UINT8_T, 20, 61, offsetof(mavlink_gps_status_t, satellite_azimuth) }, \
-         { "satellite_snr", NULL, MAVLINK_TYPE_UINT8_T, 20, 81, offsetof(mavlink_gps_status_t, satellite_snr) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a gps_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 satellites_visible Number of satellites visible
- * @param satellite_prn Global satellite ID
- * @param satellite_used 0: Satellite not used, 1: used for localization
- * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite
- * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg.
- * @param satellite_snr Signal to noise ratio of satellite
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_GPS_STATUS_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN);
-#else
-	mavlink_gps_status_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a gps_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 satellites_visible Number of satellites visible
- * @param satellite_prn Global satellite ID
- * @param satellite_used 0: Satellite not used, 1: used for localization
- * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite
- * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg.
- * @param satellite_snr Signal to noise ratio of satellite
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_GPS_STATUS_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN);
-#else
-	mavlink_gps_status_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_GPS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a gps_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 gps_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status)
-{
-	return mavlink_msg_gps_status_pack(system_id, component_id, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr);
-}
-
-/**
- * @brief Encode a gps_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 gps_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_gps_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status)
-{
-	return mavlink_msg_gps_status_pack_chan(system_id, component_id, chan, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr);
-}
-
-/**
- * @brief Send a gps_status message
- * @param chan MAVLink channel to send the message
- *
- * @param satellites_visible Number of satellites visible
- * @param satellite_prn Global satellite ID
- * @param satellite_used 0: Satellite not used, 1: used for localization
- * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite
- * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg.
- * @param satellite_snr Signal to noise ratio of satellite
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_gps_status_send(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[MAVLINK_MSG_ID_GPS_STATUS_LEN];
-	_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;
-	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
-
-// MESSAGE GPS_STATUS UNPACKING
-
-
-/**
- * @brief Get field satellites_visible from gps_status message
- *
- * @return Number of satellites visible
- */
-static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field satellite_prn from gps_status message
- *
- * @return Global satellite ID
- */
-static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, uint8_t *satellite_prn)
-{
-	return _MAV_RETURN_uint8_t_array(msg, satellite_prn, 20,  1);
-}
-
-/**
- * @brief Get field satellite_used from gps_status message
- *
- * @return 0: Satellite not used, 1: used for localization
- */
-static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, uint8_t *satellite_used)
-{
-	return _MAV_RETURN_uint8_t_array(msg, satellite_used, 20,  21);
-}
-
-/**
- * @brief Get field satellite_elevation from gps_status message
- *
- * @return Elevation (0: right on top of receiver, 90: on the horizon) of satellite
- */
-static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, uint8_t *satellite_elevation)
-{
-	return _MAV_RETURN_uint8_t_array(msg, satellite_elevation, 20,  41);
-}
-
-/**
- * @brief Get field satellite_azimuth from gps_status message
- *
- * @return Direction of satellite, 0: 0 deg, 255: 360 deg.
- */
-static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, uint8_t *satellite_azimuth)
-{
-	return _MAV_RETURN_uint8_t_array(msg, satellite_azimuth, 20,  61);
-}
-
-/**
- * @brief Get field satellite_snr from gps_status message
- *
- * @return Signal to noise ratio of satellite
- */
-static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, uint8_t *satellite_snr)
-{
-	return _MAV_RETURN_uint8_t_array(msg, satellite_snr, 20,  81);
-}
-
-/**
- * @brief Decode a gps_status message into a struct
- *
- * @param msg The message to decode
- * @param gps_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, mavlink_gps_status_t* gps_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	gps_status->satellites_visible = mavlink_msg_gps_status_get_satellites_visible(msg);
-	mavlink_msg_gps_status_get_satellite_prn(msg, gps_status->satellite_prn);
-	mavlink_msg_gps_status_get_satellite_used(msg, gps_status->satellite_used);
-	mavlink_msg_gps_status_get_satellite_elevation(msg, gps_status->satellite_elevation);
-	mavlink_msg_gps_status_get_satellite_azimuth(msg, gps_status->satellite_azimuth);
-	mavlink_msg_gps_status_get_satellite_snr(msg, gps_status->satellite_snr);
-#else
-	memcpy(gps_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_STATUS_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h
deleted file mode 100644
index 826138fad..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h
+++ /dev/null
@@ -1,284 +0,0 @@
-// MESSAGE HEARTBEAT PACKING
-
-#define MAVLINK_MSG_ID_HEARTBEAT 0
-
-typedef struct __mavlink_heartbeat_t
-{
- uint32_t custom_mode; ///< A bitfield for use for autopilot-specific flags.
- uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
- uint8_t autopilot; ///< Autopilot type / class. defined in MAV_AUTOPILOT ENUM
- uint8_t base_mode; ///< System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
- uint8_t system_status; ///< System status flag, see MAV_STATE ENUM
- uint8_t mavlink_version; ///< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version
-} mavlink_heartbeat_t;
-
-#define MAVLINK_MSG_ID_HEARTBEAT_LEN 9
-#define MAVLINK_MSG_ID_0_LEN 9
-
-#define MAVLINK_MSG_ID_HEARTBEAT_CRC 50
-#define MAVLINK_MSG_ID_0_CRC 50
-
-
-
-#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \
-	"HEARTBEAT", \
-	6, \
-	{  { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_heartbeat_t, custom_mode) }, \
-         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_heartbeat_t, type) }, \
-         { "autopilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_heartbeat_t, autopilot) }, \
-         { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_heartbeat_t, base_mode) }, \
-         { "system_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_heartbeat_t, system_status) }, \
-         { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_heartbeat_t, mavlink_version) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a heartbeat 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 type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
- * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
- * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
- * @param custom_mode A bitfield for use for autopilot-specific flags.
- * @param system_status System status flag, see MAV_STATE ENUM
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HEARTBEAT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
-#else
-	mavlink_heartbeat_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a heartbeat 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 type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
- * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
- * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
- * @param custom_mode A bitfield for use for autopilot-specific flags.
- * @param system_status System status flag, see MAV_STATE ENUM
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HEARTBEAT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
-#else
-	mavlink_heartbeat_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a heartbeat 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 heartbeat C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat)
-{
-	return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status);
-}
-
-/**
- * @brief Encode a heartbeat 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 heartbeat C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat)
-{
-	return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status);
-}
-
-/**
- * @brief Send a heartbeat message
- * @param chan MAVLink channel to send the message
- *
- * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
- * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
- * @param base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
- * @param custom_mode A bitfield for use for autopilot-specific flags.
- * @param system_status System status flag, see MAV_STATE ENUM
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_heartbeat_send(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[MAVLINK_MSG_ID_HEARTBEAT_LEN];
-	_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;
-	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
-
-// MESSAGE HEARTBEAT UNPACKING
-
-
-/**
- * @brief Get field type from heartbeat message
- *
- * @return Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
- */
-static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field autopilot from heartbeat message
- *
- * @return Autopilot type / class. defined in MAV_AUTOPILOT ENUM
- */
-static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field base_mode from heartbeat message
- *
- * @return System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
- */
-static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field custom_mode from heartbeat message
- *
- * @return A bitfield for use for autopilot-specific flags.
- */
-static inline uint32_t mavlink_msg_heartbeat_get_custom_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field system_status from heartbeat message
- *
- * @return System status flag, see MAV_STATE ENUM
- */
-static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field mavlink_version from heartbeat message
- *
- * @return MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version
- */
-static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Decode a heartbeat message into a struct
- *
- * @param msg The message to decode
- * @param heartbeat C-struct to decode the message contents into
- */
-static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	heartbeat->custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg);
-	heartbeat->type = mavlink_msg_heartbeat_get_type(msg);
-	heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg);
-	heartbeat->base_mode = mavlink_msg_heartbeat_get_base_mode(msg);
-	heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg);
-	heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg);
-#else
-	memcpy(heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HEARTBEAT_LEN);
-#endif
-}
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
deleted file mode 100644
index 0dcd95ed3..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h
+++ /dev/null
@@ -1,485 +0,0 @@
-// MESSAGE HIGHRES_IMU PACKING
-
-#define MAVLINK_MSG_ID_HIGHRES_IMU 105
-
-typedef struct __mavlink_highres_imu_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
- float xacc; ///< X acceleration (m/s^2)
- float yacc; ///< Y acceleration (m/s^2)
- float zacc; ///< Z acceleration (m/s^2)
- float xgyro; ///< Angular speed around X axis (rad / sec)
- float ygyro; ///< Angular speed around Y axis (rad / sec)
- float zgyro; ///< Angular speed around Z axis (rad / sec)
- float xmag; ///< X Magnetic field (Gauss)
- float ymag; ///< Y Magnetic field (Gauss)
- float zmag; ///< Z Magnetic field (Gauss)
- float abs_pressure; ///< Absolute pressure in millibar
- float diff_pressure; ///< Differential pressure in millibar
- float pressure_alt; ///< Altitude calculated from pressure
- float temperature; ///< Temperature in degrees celsius
- uint16_t fields_updated; ///< Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
-} mavlink_highres_imu_t;
-
-#define MAVLINK_MSG_ID_HIGHRES_IMU_LEN 62
-#define MAVLINK_MSG_ID_105_LEN 62
-
-#define MAVLINK_MSG_ID_HIGHRES_IMU_CRC 93
-#define MAVLINK_MSG_ID_105_CRC 93
-
-
-
-#define MAVLINK_MESSAGE_INFO_HIGHRES_IMU { \
-	"HIGHRES_IMU", \
-	15, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_usec) }, \
-         { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_highres_imu_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_highres_imu_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_highres_imu_t, zacc) }, \
-         { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_highres_imu_t, xgyro) }, \
-         { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_highres_imu_t, ygyro) }, \
-         { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_highres_imu_t, zgyro) }, \
-         { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_highres_imu_t, xmag) }, \
-         { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_highres_imu_t, ymag) }, \
-         { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_highres_imu_t, zmag) }, \
-         { "abs_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_highres_imu_t, abs_pressure) }, \
-         { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_highres_imu_t, diff_pressure) }, \
-         { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_highres_imu_t, pressure_alt) }, \
-         { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_highres_imu_t, temperature) }, \
-         { "fields_updated", NULL, MAVLINK_TYPE_UINT16_T, 0, 60, offsetof(mavlink_highres_imu_t, fields_updated) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a highres_imu 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_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param xacc X acceleration (m/s^2)
- * @param yacc Y acceleration (m/s^2)
- * @param zacc Z acceleration (m/s^2)
- * @param xgyro Angular speed around X axis (rad / sec)
- * @param ygyro Angular speed around Y axis (rad / sec)
- * @param zgyro Angular speed around Z axis (rad / sec)
- * @param xmag X Magnetic field (Gauss)
- * @param ymag Y Magnetic field (Gauss)
- * @param zmag Z Magnetic field (Gauss)
- * @param abs_pressure Absolute pressure in millibar
- * @param diff_pressure Differential pressure in millibar
- * @param pressure_alt Altitude calculated from pressure
- * @param temperature Temperature in degrees celsius
- * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HIGHRES_IMU_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
-#else
-	mavlink_highres_imu_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
-#endif
-}
-
-/**
- * @brief Pack a highres_imu 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_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param xacc X acceleration (m/s^2)
- * @param yacc Y acceleration (m/s^2)
- * @param zacc Z acceleration (m/s^2)
- * @param xgyro Angular speed around X axis (rad / sec)
- * @param ygyro Angular speed around Y axis (rad / sec)
- * @param zgyro Angular speed around Z axis (rad / sec)
- * @param xmag X Magnetic field (Gauss)
- * @param ymag Y Magnetic field (Gauss)
- * @param zmag Z Magnetic field (Gauss)
- * @param abs_pressure Absolute pressure in millibar
- * @param diff_pressure Differential pressure in millibar
- * @param pressure_alt Altitude calculated from pressure
- * @param temperature Temperature in degrees celsius
- * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HIGHRES_IMU_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
-#else
-	mavlink_highres_imu_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
-#endif
-}
-
-/**
- * @brief Encode a highres_imu 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 highres_imu C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu)
-{
-	return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated);
-}
-
-/**
- * @brief Encode a highres_imu 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 highres_imu C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu)
-{
-	return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated);
-}
-
-/**
- * @brief Send a highres_imu message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param xacc X acceleration (m/s^2)
- * @param yacc Y acceleration (m/s^2)
- * @param zacc Z acceleration (m/s^2)
- * @param xgyro Angular speed around X axis (rad / sec)
- * @param ygyro Angular speed around Y axis (rad / sec)
- * @param zgyro Angular speed around Z axis (rad / sec)
- * @param xmag X Magnetic field (Gauss)
- * @param ymag Y Magnetic field (Gauss)
- * @param zmag Z Magnetic field (Gauss)
- * @param abs_pressure Absolute pressure in millibar
- * @param diff_pressure Differential pressure in millibar
- * @param pressure_alt Altitude calculated from pressure
- * @param temperature Temperature in degrees celsius
- * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_highres_imu_send(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[MAVLINK_MSG_ID_HIGHRES_IMU_LEN];
-	_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;
-	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
-
-// MESSAGE HIGHRES_IMU UNPACKING
-
-
-/**
- * @brief Get field time_usec from highres_imu message
- *
- * @return Timestamp (microseconds, synced to UNIX time or since system boot)
- */
-static inline uint64_t mavlink_msg_highres_imu_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field xacc from highres_imu message
- *
- * @return X acceleration (m/s^2)
- */
-static inline float mavlink_msg_highres_imu_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yacc from highres_imu message
- *
- * @return Y acceleration (m/s^2)
- */
-static inline float mavlink_msg_highres_imu_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field zacc from highres_imu message
- *
- * @return Z acceleration (m/s^2)
- */
-static inline float mavlink_msg_highres_imu_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field xgyro from highres_imu message
- *
- * @return Angular speed around X axis (rad / sec)
- */
-static inline float mavlink_msg_highres_imu_get_xgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field ygyro from highres_imu message
- *
- * @return Angular speed around Y axis (rad / sec)
- */
-static inline float mavlink_msg_highres_imu_get_ygyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field zgyro from highres_imu message
- *
- * @return Angular speed around Z axis (rad / sec)
- */
-static inline float mavlink_msg_highres_imu_get_zgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field xmag from highres_imu message
- *
- * @return X Magnetic field (Gauss)
- */
-static inline float mavlink_msg_highres_imu_get_xmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field ymag from highres_imu message
- *
- * @return Y Magnetic field (Gauss)
- */
-static inline float mavlink_msg_highres_imu_get_ymag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  36);
-}
-
-/**
- * @brief Get field zmag from highres_imu message
- *
- * @return Z Magnetic field (Gauss)
- */
-static inline float mavlink_msg_highres_imu_get_zmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  40);
-}
-
-/**
- * @brief Get field abs_pressure from highres_imu message
- *
- * @return Absolute pressure in millibar
- */
-static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  44);
-}
-
-/**
- * @brief Get field diff_pressure from highres_imu message
- *
- * @return Differential pressure in millibar
- */
-static inline float mavlink_msg_highres_imu_get_diff_pressure(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  48);
-}
-
-/**
- * @brief Get field pressure_alt from highres_imu message
- *
- * @return Altitude calculated from pressure
- */
-static inline float mavlink_msg_highres_imu_get_pressure_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  52);
-}
-
-/**
- * @brief Get field temperature from highres_imu message
- *
- * @return Temperature in degrees celsius
- */
-static inline float mavlink_msg_highres_imu_get_temperature(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  56);
-}
-
-/**
- * @brief Get field fields_updated from highres_imu message
- *
- * @return Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
- */
-static inline uint16_t mavlink_msg_highres_imu_get_fields_updated(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  60);
-}
-
-/**
- * @brief Decode a highres_imu message into a struct
- *
- * @param msg The message to decode
- * @param highres_imu C-struct to decode the message contents into
- */
-static inline void mavlink_msg_highres_imu_decode(const mavlink_message_t* msg, mavlink_highres_imu_t* highres_imu)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	highres_imu->time_usec = mavlink_msg_highres_imu_get_time_usec(msg);
-	highres_imu->xacc = mavlink_msg_highres_imu_get_xacc(msg);
-	highres_imu->yacc = mavlink_msg_highres_imu_get_yacc(msg);
-	highres_imu->zacc = mavlink_msg_highres_imu_get_zacc(msg);
-	highres_imu->xgyro = mavlink_msg_highres_imu_get_xgyro(msg);
-	highres_imu->ygyro = mavlink_msg_highres_imu_get_ygyro(msg);
-	highres_imu->zgyro = mavlink_msg_highres_imu_get_zgyro(msg);
-	highres_imu->xmag = mavlink_msg_highres_imu_get_xmag(msg);
-	highres_imu->ymag = mavlink_msg_highres_imu_get_ymag(msg);
-	highres_imu->zmag = mavlink_msg_highres_imu_get_zmag(msg);
-	highres_imu->abs_pressure = mavlink_msg_highres_imu_get_abs_pressure(msg);
-	highres_imu->diff_pressure = mavlink_msg_highres_imu_get_diff_pressure(msg);
-	highres_imu->pressure_alt = mavlink_msg_highres_imu_get_pressure_alt(msg);
-	highres_imu->temperature = mavlink_msg_highres_imu_get_temperature(msg);
-	highres_imu->fields_updated = mavlink_msg_highres_imu_get_fields_updated(msg);
-#else
-	memcpy(highres_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
-#endif
-}
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
deleted file mode 100644
index aed5108d0..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h
+++ /dev/null
@@ -1,397 +0,0 @@
-// MESSAGE HIL_CONTROLS PACKING
-
-#define MAVLINK_MSG_ID_HIL_CONTROLS 91
-
-typedef struct __mavlink_hil_controls_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- float roll_ailerons; ///< Control output -1 .. 1
- float pitch_elevator; ///< Control output -1 .. 1
- float yaw_rudder; ///< Control output -1 .. 1
- float throttle; ///< Throttle 0 .. 1
- float aux1; ///< Aux 1, -1 .. 1
- float aux2; ///< Aux 2, -1 .. 1
- float aux3; ///< Aux 3, -1 .. 1
- float aux4; ///< Aux 4, -1 .. 1
- uint8_t mode; ///< System mode (MAV_MODE)
- uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE)
-} mavlink_hil_controls_t;
-
-#define MAVLINK_MSG_ID_HIL_CONTROLS_LEN 42
-#define MAVLINK_MSG_ID_91_LEN 42
-
-#define MAVLINK_MSG_ID_HIL_CONTROLS_CRC 63
-#define MAVLINK_MSG_ID_91_CRC 63
-
-
-
-#define MAVLINK_MESSAGE_INFO_HIL_CONTROLS { \
-	"HIL_CONTROLS", \
-	11, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_controls_t, time_usec) }, \
-         { "roll_ailerons", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_controls_t, roll_ailerons) }, \
-         { "pitch_elevator", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_controls_t, pitch_elevator) }, \
-         { "yaw_rudder", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_controls_t, yaw_rudder) }, \
-         { "throttle", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_controls_t, throttle) }, \
-         { "aux1", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_controls_t, aux1) }, \
-         { "aux2", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_controls_t, aux2) }, \
-         { "aux3", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_controls_t, aux3) }, \
-         { "aux4", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_controls_t, aux4) }, \
-         { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_hil_controls_t, mode) }, \
-         { "nav_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_hil_controls_t, nav_mode) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hil_controls 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param roll_ailerons Control output -1 .. 1
- * @param pitch_elevator Control output -1 .. 1
- * @param yaw_rudder Control output -1 .. 1
- * @param throttle Throttle 0 .. 1
- * @param aux1 Aux 1, -1 .. 1
- * @param aux2 Aux 2, -1 .. 1
- * @param aux3 Aux 3, -1 .. 1
- * @param aux4 Aux 4, -1 .. 1
- * @param mode System mode (MAV_MODE)
- * @param nav_mode Navigation mode (MAV_NAV_MODE)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HIL_CONTROLS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
-#else
-	mavlink_hil_controls_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hil_controls 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param roll_ailerons Control output -1 .. 1
- * @param pitch_elevator Control output -1 .. 1
- * @param yaw_rudder Control output -1 .. 1
- * @param throttle Throttle 0 .. 1
- * @param aux1 Aux 1, -1 .. 1
- * @param aux2 Aux 2, -1 .. 1
- * @param aux3 Aux 3, -1 .. 1
- * @param aux4 Aux 4, -1 .. 1
- * @param mode System mode (MAV_MODE)
- * @param nav_mode Navigation mode (MAV_NAV_MODE)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HIL_CONTROLS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
-#else
-	mavlink_hil_controls_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hil_controls 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 hil_controls C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls)
-{
-	return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode);
-}
-
-/**
- * @brief Encode a hil_controls 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 hil_controls C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_controls_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls)
-{
-	return mavlink_msg_hil_controls_pack_chan(system_id, component_id, chan, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode);
-}
-
-/**
- * @brief Send a hil_controls message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param roll_ailerons Control output -1 .. 1
- * @param pitch_elevator Control output -1 .. 1
- * @param yaw_rudder Control output -1 .. 1
- * @param throttle Throttle 0 .. 1
- * @param aux1 Aux 1, -1 .. 1
- * @param aux2 Aux 2, -1 .. 1
- * @param aux3 Aux 3, -1 .. 1
- * @param aux4 Aux 4, -1 .. 1
- * @param mode System mode (MAV_MODE)
- * @param nav_mode Navigation mode (MAV_NAV_MODE)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hil_controls_send(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[MAVLINK_MSG_ID_HIL_CONTROLS_LEN];
-	_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;
-	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
-
-// MESSAGE HIL_CONTROLS UNPACKING
-
-
-/**
- * @brief Get field time_usec from hil_controls message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_hil_controls_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field roll_ailerons from hil_controls message
- *
- * @return Control output -1 .. 1
- */
-static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field pitch_elevator from hil_controls message
- *
- * @return Control output -1 .. 1
- */
-static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field yaw_rudder from hil_controls message
- *
- * @return Control output -1 .. 1
- */
-static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field throttle from hil_controls message
- *
- * @return Throttle 0 .. 1
- */
-static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field aux1 from hil_controls message
- *
- * @return Aux 1, -1 .. 1
- */
-static inline float mavlink_msg_hil_controls_get_aux1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field aux2 from hil_controls message
- *
- * @return Aux 2, -1 .. 1
- */
-static inline float mavlink_msg_hil_controls_get_aux2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field aux3 from hil_controls message
- *
- * @return Aux 3, -1 .. 1
- */
-static inline float mavlink_msg_hil_controls_get_aux3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field aux4 from hil_controls message
- *
- * @return Aux 4, -1 .. 1
- */
-static inline float mavlink_msg_hil_controls_get_aux4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  36);
-}
-
-/**
- * @brief Get field mode from hil_controls message
- *
- * @return System mode (MAV_MODE)
- */
-static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  40);
-}
-
-/**
- * @brief Get field nav_mode from hil_controls message
- *
- * @return Navigation mode (MAV_NAV_MODE)
- */
-static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  41);
-}
-
-/**
- * @brief Decode a hil_controls message into a struct
- *
- * @param msg The message to decode
- * @param hil_controls C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, mavlink_hil_controls_t* hil_controls)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hil_controls->time_usec = mavlink_msg_hil_controls_get_time_usec(msg);
-	hil_controls->roll_ailerons = mavlink_msg_hil_controls_get_roll_ailerons(msg);
-	hil_controls->pitch_elevator = mavlink_msg_hil_controls_get_pitch_elevator(msg);
-	hil_controls->yaw_rudder = mavlink_msg_hil_controls_get_yaw_rudder(msg);
-	hil_controls->throttle = mavlink_msg_hil_controls_get_throttle(msg);
-	hil_controls->aux1 = mavlink_msg_hil_controls_get_aux1(msg);
-	hil_controls->aux2 = mavlink_msg_hil_controls_get_aux2(msg);
-	hil_controls->aux3 = mavlink_msg_hil_controls_get_aux3(msg);
-	hil_controls->aux4 = mavlink_msg_hil_controls_get_aux4(msg);
-	hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg);
-	hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg);
-#else
-	memcpy(hil_controls, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
-#endif
-}
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
deleted file mode 100644
index 91aec5b08..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h
+++ /dev/null
@@ -1,441 +0,0 @@
-// MESSAGE HIL_GPS PACKING
-
-#define MAVLINK_MSG_ID_HIL_GPS 113
-
-typedef struct __mavlink_hil_gps_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- int32_t lat; ///< Latitude (WGS84), in degrees * 1E7
- int32_t lon; ///< Longitude (WGS84), in degrees * 1E7
- int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up)
- uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
- uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
- uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535
- int16_t vn; ///< GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
- int16_t ve; ///< GPS velocity in cm/s in EAST direction in earth-fixed NED frame
- int16_t vd; ///< GPS velocity in cm/s in DOWN direction in earth-fixed NED frame
- uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
- 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.
- uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255
-} mavlink_hil_gps_t;
-
-#define MAVLINK_MSG_ID_HIL_GPS_LEN 36
-#define MAVLINK_MSG_ID_113_LEN 36
-
-#define MAVLINK_MSG_ID_HIL_GPS_CRC 124
-#define MAVLINK_MSG_ID_113_CRC 124
-
-
-
-#define MAVLINK_MESSAGE_INFO_HIL_GPS { \
-	"HIL_GPS", \
-	13, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_gps_t, time_usec) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_hil_gps_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_hil_gps_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_hil_gps_t, alt) }, \
-         { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_hil_gps_t, eph) }, \
-         { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_hil_gps_t, epv) }, \
-         { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_hil_gps_t, vel) }, \
-         { "vn", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_hil_gps_t, vn) }, \
-         { "ve", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_hil_gps_t, ve) }, \
-         { "vd", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_hil_gps_t, vd) }, \
-         { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_hil_gps_t, cog) }, \
-         { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_hil_gps_t, fix_type) }, \
-         { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_hil_gps_t, satellites_visible) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hil_gps 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535
- * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
- * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame
- * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HIL_GPS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN);
-#else
-	mavlink_hil_gps_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_GPS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hil_gps 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535
- * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
- * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame
- * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HIL_GPS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN);
-#else
-	mavlink_hil_gps_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_GPS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_GPS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hil_gps 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 hil_gps C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps)
-{
-	return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible);
-}
-
-/**
- * @brief Encode a hil_gps 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 hil_gps C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps)
-{
-	return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible);
-}
-
-/**
- * @brief Send a hil_gps message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param 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.
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (WGS84), in meters * 1000 (positive for up)
- * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
- * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
- * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535
- * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
- * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame
- * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame
- * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
- * @param satellites_visible Number of satellites visible. If unknown, set to 255
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hil_gps_send(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[MAVLINK_MSG_ID_HIL_GPS_LEN];
-	_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;
-	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
-
-// MESSAGE HIL_GPS UNPACKING
-
-
-/**
- * @brief Get field time_usec from hil_gps message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_hil_gps_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field fix_type from hil_gps message
- *
- * @return 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.
- */
-static inline uint8_t mavlink_msg_hil_gps_get_fix_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  34);
-}
-
-/**
- * @brief Get field lat from hil_gps message
- *
- * @return Latitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_hil_gps_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field lon from hil_gps message
- *
- * @return Longitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_hil_gps_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  12);
-}
-
-/**
- * @brief Get field alt from hil_gps message
- *
- * @return Altitude (WGS84), in meters * 1000 (positive for up)
- */
-static inline int32_t mavlink_msg_hil_gps_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  16);
-}
-
-/**
- * @brief Get field eph from hil_gps message
- *
- * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
- */
-static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  20);
-}
-
-/**
- * @brief Get field epv from hil_gps message
- *
- * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
- */
-static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  22);
-}
-
-/**
- * @brief Get field vel from hil_gps message
- *
- * @return GPS ground speed (m/s * 100). If unknown, set to: 65535
- */
-static inline uint16_t mavlink_msg_hil_gps_get_vel(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  24);
-}
-
-/**
- * @brief Get field vn from hil_gps message
- *
- * @return GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
- */
-static inline int16_t mavlink_msg_hil_gps_get_vn(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  26);
-}
-
-/**
- * @brief Get field ve from hil_gps message
- *
- * @return GPS velocity in cm/s in EAST direction in earth-fixed NED frame
- */
-static inline int16_t mavlink_msg_hil_gps_get_ve(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  28);
-}
-
-/**
- * @brief Get field vd from hil_gps message
- *
- * @return GPS velocity in cm/s in DOWN direction in earth-fixed NED frame
- */
-static inline int16_t mavlink_msg_hil_gps_get_vd(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  30);
-}
-
-/**
- * @brief Get field cog from hil_gps message
- *
- * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
- */
-static inline uint16_t mavlink_msg_hil_gps_get_cog(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  32);
-}
-
-/**
- * @brief Get field satellites_visible from hil_gps message
- *
- * @return Number of satellites visible. If unknown, set to 255
- */
-static inline uint8_t mavlink_msg_hil_gps_get_satellites_visible(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  35);
-}
-
-/**
- * @brief Decode a hil_gps message into a struct
- *
- * @param msg The message to decode
- * @param hil_gps C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hil_gps_decode(const mavlink_message_t* msg, mavlink_hil_gps_t* hil_gps)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hil_gps->time_usec = mavlink_msg_hil_gps_get_time_usec(msg);
-	hil_gps->lat = mavlink_msg_hil_gps_get_lat(msg);
-	hil_gps->lon = mavlink_msg_hil_gps_get_lon(msg);
-	hil_gps->alt = mavlink_msg_hil_gps_get_alt(msg);
-	hil_gps->eph = mavlink_msg_hil_gps_get_eph(msg);
-	hil_gps->epv = mavlink_msg_hil_gps_get_epv(msg);
-	hil_gps->vel = mavlink_msg_hil_gps_get_vel(msg);
-	hil_gps->vn = mavlink_msg_hil_gps_get_vn(msg);
-	hil_gps->ve = mavlink_msg_hil_gps_get_ve(msg);
-	hil_gps->vd = mavlink_msg_hil_gps_get_vd(msg);
-	hil_gps->cog = mavlink_msg_hil_gps_get_cog(msg);
-	hil_gps->fix_type = mavlink_msg_hil_gps_get_fix_type(msg);
-	hil_gps->satellites_visible = mavlink_msg_hil_gps_get_satellites_visible(msg);
-#else
-	memcpy(hil_gps, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_GPS_LEN);
-#endif
-}
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
deleted file mode 100644
index acb1392e1..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h
+++ /dev/null
@@ -1,331 +0,0 @@
-// MESSAGE HIL_OPTICAL_FLOW PACKING
-
-#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW 114
-
-typedef struct __mavlink_hil_optical_flow_t
-{
- uint64_t time_usec; ///< Timestamp (UNIX)
- float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated
- float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated
- float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- int16_t flow_x; ///< Flow in pixels in x-sensor direction
- int16_t flow_y; ///< Flow in pixels in y-sensor direction
- uint8_t sensor_id; ///< Sensor ID
- uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality
-} mavlink_hil_optical_flow_t;
-
-#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN 26
-#define MAVLINK_MSG_ID_114_LEN 26
-
-#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC 119
-#define MAVLINK_MSG_ID_114_CRC 119
-
-
-
-#define MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW { \
-	"HIL_OPTICAL_FLOW", \
-	8, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_optical_flow_t, time_usec) }, \
-         { "flow_comp_m_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_optical_flow_t, flow_comp_m_x) }, \
-         { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_optical_flow_t, flow_comp_m_y) }, \
-         { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_optical_flow_t, ground_distance) }, \
-         { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_hil_optical_flow_t, flow_x) }, \
-         { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_hil_optical_flow_t, flow_y) }, \
-         { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_hil_optical_flow_t, sensor_id) }, \
-         { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_hil_optical_flow_t, quality) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hil_optical_flow 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_usec Timestamp (UNIX)
- * @param sensor_id Sensor ID
- * @param flow_x Flow in pixels in x-sensor direction
- * @param flow_y Flow in pixels in y-sensor direction
- * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated
- * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
-#else
-	mavlink_hil_optical_flow_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hil_optical_flow 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_usec Timestamp (UNIX)
- * @param sensor_id Sensor ID
- * @param flow_x Flow in pixels in x-sensor direction
- * @param flow_y Flow in pixels in y-sensor direction
- * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated
- * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
-#else
-	mavlink_hil_optical_flow_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hil_optical_flow 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 hil_optical_flow C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow)
-{
-	return mavlink_msg_hil_optical_flow_pack(system_id, component_id, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance);
-}
-
-/**
- * @brief Encode a hil_optical_flow 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 hil_optical_flow C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow)
-{
-	return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance);
-}
-
-/**
- * @brief Send a hil_optical_flow message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (UNIX)
- * @param sensor_id Sensor ID
- * @param flow_x Flow in pixels in x-sensor direction
- * @param flow_y Flow in pixels in y-sensor direction
- * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated
- * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hil_optical_flow_send(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[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN];
-	_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;
-	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
-
-// MESSAGE HIL_OPTICAL_FLOW UNPACKING
-
-
-/**
- * @brief Get field time_usec from hil_optical_flow message
- *
- * @return Timestamp (UNIX)
- */
-static inline uint64_t mavlink_msg_hil_optical_flow_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field sensor_id from hil_optical_flow message
- *
- * @return Sensor ID
- */
-static inline uint8_t mavlink_msg_hil_optical_flow_get_sensor_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  24);
-}
-
-/**
- * @brief Get field flow_x from hil_optical_flow message
- *
- * @return Flow in pixels in x-sensor direction
- */
-static inline int16_t mavlink_msg_hil_optical_flow_get_flow_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Get field flow_y from hil_optical_flow message
- *
- * @return Flow in pixels in y-sensor direction
- */
-static inline int16_t mavlink_msg_hil_optical_flow_get_flow_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  22);
-}
-
-/**
- * @brief Get field flow_comp_m_x from hil_optical_flow message
- *
- * @return Flow in meters in x-sensor direction, angular-speed compensated
- */
-static inline float mavlink_msg_hil_optical_flow_get_flow_comp_m_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field flow_comp_m_y from hil_optical_flow message
- *
- * @return Flow in meters in y-sensor direction, angular-speed compensated
- */
-static inline float mavlink_msg_hil_optical_flow_get_flow_comp_m_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field quality from hil_optical_flow message
- *
- * @return Optical flow quality / confidence. 0: bad, 255: maximum quality
- */
-static inline uint8_t mavlink_msg_hil_optical_flow_get_quality(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  25);
-}
-
-/**
- * @brief Get field ground_distance from hil_optical_flow message
- *
- * @return Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- */
-static inline float mavlink_msg_hil_optical_flow_get_ground_distance(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a hil_optical_flow message into a struct
- *
- * @param msg The message to decode
- * @param hil_optical_flow C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hil_optical_flow_decode(const mavlink_message_t* msg, mavlink_hil_optical_flow_t* hil_optical_flow)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hil_optical_flow->time_usec = mavlink_msg_hil_optical_flow_get_time_usec(msg);
-	hil_optical_flow->flow_comp_m_x = mavlink_msg_hil_optical_flow_get_flow_comp_m_x(msg);
-	hil_optical_flow->flow_comp_m_y = mavlink_msg_hil_optical_flow_get_flow_comp_m_y(msg);
-	hil_optical_flow->ground_distance = mavlink_msg_hil_optical_flow_get_ground_distance(msg);
-	hil_optical_flow->flow_x = mavlink_msg_hil_optical_flow_get_flow_x(msg);
-	hil_optical_flow->flow_y = mavlink_msg_hil_optical_flow_get_flow_y(msg);
-	hil_optical_flow->sensor_id = mavlink_msg_hil_optical_flow_get_sensor_id(msg);
-	hil_optical_flow->quality = mavlink_msg_hil_optical_flow_get_quality(msg);
-#else
-	memcpy(hil_optical_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
-#endif
-}
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
deleted file mode 100644
index a42bde50b..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h
+++ /dev/null
@@ -1,463 +0,0 @@
-// MESSAGE HIL_RC_INPUTS_RAW PACKING
-
-#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW 92
-
-typedef struct __mavlink_hil_rc_inputs_raw_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- uint16_t chan1_raw; ///< RC channel 1 value, in microseconds
- uint16_t chan2_raw; ///< RC channel 2 value, in microseconds
- uint16_t chan3_raw; ///< RC channel 3 value, in microseconds
- uint16_t chan4_raw; ///< RC channel 4 value, in microseconds
- uint16_t chan5_raw; ///< RC channel 5 value, in microseconds
- uint16_t chan6_raw; ///< RC channel 6 value, in microseconds
- uint16_t chan7_raw; ///< RC channel 7 value, in microseconds
- uint16_t chan8_raw; ///< RC channel 8 value, in microseconds
- uint16_t chan9_raw; ///< RC channel 9 value, in microseconds
- uint16_t chan10_raw; ///< RC channel 10 value, in microseconds
- uint16_t chan11_raw; ///< RC channel 11 value, in microseconds
- uint16_t chan12_raw; ///< RC channel 12 value, in microseconds
- uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100%
-} mavlink_hil_rc_inputs_raw_t;
-
-#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN 33
-#define MAVLINK_MSG_ID_92_LEN 33
-
-#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC 54
-#define MAVLINK_MSG_ID_92_CRC 54
-
-
-
-#define MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW { \
-	"HIL_RC_INPUTS_RAW", \
-	14, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_rc_inputs_raw_t, time_usec) }, \
-         { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_hil_rc_inputs_raw_t, chan1_raw) }, \
-         { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_hil_rc_inputs_raw_t, chan2_raw) }, \
-         { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_hil_rc_inputs_raw_t, chan3_raw) }, \
-         { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_hil_rc_inputs_raw_t, chan4_raw) }, \
-         { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_hil_rc_inputs_raw_t, chan5_raw) }, \
-         { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_hil_rc_inputs_raw_t, chan6_raw) }, \
-         { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_hil_rc_inputs_raw_t, chan7_raw) }, \
-         { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_hil_rc_inputs_raw_t, chan8_raw) }, \
-         { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_hil_rc_inputs_raw_t, chan9_raw) }, \
-         { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_hil_rc_inputs_raw_t, chan10_raw) }, \
-         { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_hil_rc_inputs_raw_t, chan11_raw) }, \
-         { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_hil_rc_inputs_raw_t, chan12_raw) }, \
-         { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_hil_rc_inputs_raw_t, rssi) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hil_rc_inputs_raw 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param chan1_raw RC channel 1 value, in microseconds
- * @param chan2_raw RC channel 2 value, in microseconds
- * @param chan3_raw RC channel 3 value, in microseconds
- * @param chan4_raw RC channel 4 value, in microseconds
- * @param chan5_raw RC channel 5 value, in microseconds
- * @param chan6_raw RC channel 6 value, in microseconds
- * @param chan7_raw RC channel 7 value, in microseconds
- * @param chan8_raw RC channel 8 value, in microseconds
- * @param chan9_raw RC channel 9 value, in microseconds
- * @param chan10_raw RC channel 10 value, in microseconds
- * @param chan11_raw RC channel 11 value, in microseconds
- * @param chan12_raw RC channel 12 value, in microseconds
- * @param rssi Receive signal strength indicator, 0: 0%, 255: 100%
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
-#else
-	mavlink_hil_rc_inputs_raw_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hil_rc_inputs_raw 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param chan1_raw RC channel 1 value, in microseconds
- * @param chan2_raw RC channel 2 value, in microseconds
- * @param chan3_raw RC channel 3 value, in microseconds
- * @param chan4_raw RC channel 4 value, in microseconds
- * @param chan5_raw RC channel 5 value, in microseconds
- * @param chan6_raw RC channel 6 value, in microseconds
- * @param chan7_raw RC channel 7 value, in microseconds
- * @param chan8_raw RC channel 8 value, in microseconds
- * @param chan9_raw RC channel 9 value, in microseconds
- * @param chan10_raw RC channel 10 value, in microseconds
- * @param chan11_raw RC channel 11 value, in microseconds
- * @param chan12_raw RC channel 12 value, in microseconds
- * @param rssi Receive signal strength indicator, 0: 0%, 255: 100%
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
-#else
-	mavlink_hil_rc_inputs_raw_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hil_rc_inputs_raw 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 hil_rc_inputs_raw C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw)
-{
-	return mavlink_msg_hil_rc_inputs_raw_pack(system_id, component_id, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi);
-}
-
-/**
- * @brief Encode a hil_rc_inputs_raw 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 hil_rc_inputs_raw C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw)
-{
-	return mavlink_msg_hil_rc_inputs_raw_pack_chan(system_id, component_id, chan, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi);
-}
-
-/**
- * @brief Send a hil_rc_inputs_raw message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param chan1_raw RC channel 1 value, in microseconds
- * @param chan2_raw RC channel 2 value, in microseconds
- * @param chan3_raw RC channel 3 value, in microseconds
- * @param chan4_raw RC channel 4 value, in microseconds
- * @param chan5_raw RC channel 5 value, in microseconds
- * @param chan6_raw RC channel 6 value, in microseconds
- * @param chan7_raw RC channel 7 value, in microseconds
- * @param chan8_raw RC channel 8 value, in microseconds
- * @param chan9_raw RC channel 9 value, in microseconds
- * @param chan10_raw RC channel 10 value, in microseconds
- * @param chan11_raw RC channel 11 value, in microseconds
- * @param chan12_raw RC channel 12 value, in microseconds
- * @param rssi Receive signal strength indicator, 0: 0%, 255: 100%
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hil_rc_inputs_raw_send(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[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN];
-	_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;
-	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
-
-// MESSAGE HIL_RC_INPUTS_RAW UNPACKING
-
-
-/**
- * @brief Get field time_usec from hil_rc_inputs_raw message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_hil_rc_inputs_raw_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field chan1_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 1 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field chan2_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 2 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Get field chan3_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 3 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field chan4_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 4 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  14);
-}
-
-/**
- * @brief Get field chan5_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 5 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field chan6_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 6 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  18);
-}
-
-/**
- * @brief Get field chan7_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 7 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  20);
-}
-
-/**
- * @brief Get field chan8_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 8 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  22);
-}
-
-/**
- * @brief Get field chan9_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 9 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  24);
-}
-
-/**
- * @brief Get field chan10_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 10 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  26);
-}
-
-/**
- * @brief Get field chan11_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 11 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  28);
-}
-
-/**
- * @brief Get field chan12_raw from hil_rc_inputs_raw message
- *
- * @return RC channel 12 value, in microseconds
- */
-static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  30);
-}
-
-/**
- * @brief Get field rssi from hil_rc_inputs_raw message
- *
- * @return Receive signal strength indicator, 0: 0%, 255: 100%
- */
-static inline uint8_t mavlink_msg_hil_rc_inputs_raw_get_rssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  32);
-}
-
-/**
- * @brief Decode a hil_rc_inputs_raw message into a struct
- *
- * @param msg The message to decode
- * @param hil_rc_inputs_raw C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hil_rc_inputs_raw_decode(const mavlink_message_t* msg, mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hil_rc_inputs_raw->time_usec = mavlink_msg_hil_rc_inputs_raw_get_time_usec(msg);
-	hil_rc_inputs_raw->chan1_raw = mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(msg);
-	hil_rc_inputs_raw->chan2_raw = mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(msg);
-	hil_rc_inputs_raw->chan3_raw = mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(msg);
-	hil_rc_inputs_raw->chan4_raw = mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(msg);
-	hil_rc_inputs_raw->chan5_raw = mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(msg);
-	hil_rc_inputs_raw->chan6_raw = mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(msg);
-	hil_rc_inputs_raw->chan7_raw = mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(msg);
-	hil_rc_inputs_raw->chan8_raw = mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(msg);
-	hil_rc_inputs_raw->chan9_raw = mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(msg);
-	hil_rc_inputs_raw->chan10_raw = mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(msg);
-	hil_rc_inputs_raw->chan11_raw = mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(msg);
-	hil_rc_inputs_raw->chan12_raw = mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(msg);
-	hil_rc_inputs_raw->rssi = mavlink_msg_hil_rc_inputs_raw_get_rssi(msg);
-#else
-	memcpy(hil_rc_inputs_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
-#endif
-}
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
deleted file mode 100644
index 6c2667473..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h
+++ /dev/null
@@ -1,485 +0,0 @@
-// MESSAGE HIL_SENSOR PACKING
-
-#define MAVLINK_MSG_ID_HIL_SENSOR 107
-
-typedef struct __mavlink_hil_sensor_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
- float xacc; ///< X acceleration (m/s^2)
- float yacc; ///< Y acceleration (m/s^2)
- float zacc; ///< Z acceleration (m/s^2)
- float xgyro; ///< Angular speed around X axis in body frame (rad / sec)
- float ygyro; ///< Angular speed around Y axis in body frame (rad / sec)
- float zgyro; ///< Angular speed around Z axis in body frame (rad / sec)
- float xmag; ///< X Magnetic field (Gauss)
- float ymag; ///< Y Magnetic field (Gauss)
- float zmag; ///< Z Magnetic field (Gauss)
- float abs_pressure; ///< Absolute pressure in millibar
- float diff_pressure; ///< Differential pressure (airspeed) in millibar
- float pressure_alt; ///< Altitude calculated from pressure
- float temperature; ///< Temperature in degrees celsius
- uint32_t fields_updated; ///< Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
-} mavlink_hil_sensor_t;
-
-#define MAVLINK_MSG_ID_HIL_SENSOR_LEN 64
-#define MAVLINK_MSG_ID_107_LEN 64
-
-#define MAVLINK_MSG_ID_HIL_SENSOR_CRC 108
-#define MAVLINK_MSG_ID_107_CRC 108
-
-
-
-#define MAVLINK_MESSAGE_INFO_HIL_SENSOR { \
-	"HIL_SENSOR", \
-	15, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_sensor_t, time_usec) }, \
-         { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_sensor_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_sensor_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_sensor_t, zacc) }, \
-         { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_sensor_t, xgyro) }, \
-         { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_sensor_t, ygyro) }, \
-         { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_sensor_t, zgyro) }, \
-         { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_sensor_t, xmag) }, \
-         { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_sensor_t, ymag) }, \
-         { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_hil_sensor_t, zmag) }, \
-         { "abs_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_hil_sensor_t, abs_pressure) }, \
-         { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_hil_sensor_t, diff_pressure) }, \
-         { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_hil_sensor_t, pressure_alt) }, \
-         { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_hil_sensor_t, temperature) }, \
-         { "fields_updated", NULL, MAVLINK_TYPE_UINT32_T, 0, 60, offsetof(mavlink_hil_sensor_t, fields_updated) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hil_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_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param xacc X acceleration (m/s^2)
- * @param yacc Y acceleration (m/s^2)
- * @param zacc Z acceleration (m/s^2)
- * @param xgyro Angular speed around X axis in body frame (rad / sec)
- * @param ygyro Angular speed around Y axis in body frame (rad / sec)
- * @param zgyro Angular speed around Z axis in body frame (rad / sec)
- * @param xmag X Magnetic field (Gauss)
- * @param ymag Y Magnetic field (Gauss)
- * @param zmag Z Magnetic field (Gauss)
- * @param abs_pressure Absolute pressure in millibar
- * @param diff_pressure Differential pressure (airspeed) in millibar
- * @param pressure_alt Altitude calculated from pressure
- * @param temperature Temperature in degrees celsius
- * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HIL_SENSOR_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN);
-#else
-	mavlink_hil_sensor_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hil_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_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param xacc X acceleration (m/s^2)
- * @param yacc Y acceleration (m/s^2)
- * @param zacc Z acceleration (m/s^2)
- * @param xgyro Angular speed around X axis in body frame (rad / sec)
- * @param ygyro Angular speed around Y axis in body frame (rad / sec)
- * @param zgyro Angular speed around Z axis in body frame (rad / sec)
- * @param xmag X Magnetic field (Gauss)
- * @param ymag Y Magnetic field (Gauss)
- * @param zmag Z Magnetic field (Gauss)
- * @param abs_pressure Absolute pressure in millibar
- * @param diff_pressure Differential pressure (airspeed) in millibar
- * @param pressure_alt Altitude calculated from pressure
- * @param temperature Temperature in degrees celsius
- * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HIL_SENSOR_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN);
-#else
-	mavlink_hil_sensor_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_SENSOR_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hil_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 hil_sensor C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor)
-{
-	return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated);
-}
-
-/**
- * @brief Encode a hil_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 hil_sensor C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor)
-{
-	return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated);
-}
-
-/**
- * @brief Send a hil_sensor message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param xacc X acceleration (m/s^2)
- * @param yacc Y acceleration (m/s^2)
- * @param zacc Z acceleration (m/s^2)
- * @param xgyro Angular speed around X axis in body frame (rad / sec)
- * @param ygyro Angular speed around Y axis in body frame (rad / sec)
- * @param zgyro Angular speed around Z axis in body frame (rad / sec)
- * @param xmag X Magnetic field (Gauss)
- * @param ymag Y Magnetic field (Gauss)
- * @param zmag Z Magnetic field (Gauss)
- * @param abs_pressure Absolute pressure in millibar
- * @param diff_pressure Differential pressure (airspeed) in millibar
- * @param pressure_alt Altitude calculated from pressure
- * @param temperature Temperature in degrees celsius
- * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hil_sensor_send(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[MAVLINK_MSG_ID_HIL_SENSOR_LEN];
-	_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;
-	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
-
-// MESSAGE HIL_SENSOR UNPACKING
-
-
-/**
- * @brief Get field time_usec from hil_sensor message
- *
- * @return Timestamp (microseconds, synced to UNIX time or since system boot)
- */
-static inline uint64_t mavlink_msg_hil_sensor_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field xacc from hil_sensor message
- *
- * @return X acceleration (m/s^2)
- */
-static inline float mavlink_msg_hil_sensor_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yacc from hil_sensor message
- *
- * @return Y acceleration (m/s^2)
- */
-static inline float mavlink_msg_hil_sensor_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field zacc from hil_sensor message
- *
- * @return Z acceleration (m/s^2)
- */
-static inline float mavlink_msg_hil_sensor_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field xgyro from hil_sensor message
- *
- * @return Angular speed around X axis in body frame (rad / sec)
- */
-static inline float mavlink_msg_hil_sensor_get_xgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field ygyro from hil_sensor message
- *
- * @return Angular speed around Y axis in body frame (rad / sec)
- */
-static inline float mavlink_msg_hil_sensor_get_ygyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field zgyro from hil_sensor message
- *
- * @return Angular speed around Z axis in body frame (rad / sec)
- */
-static inline float mavlink_msg_hil_sensor_get_zgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field xmag from hil_sensor message
- *
- * @return X Magnetic field (Gauss)
- */
-static inline float mavlink_msg_hil_sensor_get_xmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field ymag from hil_sensor message
- *
- * @return Y Magnetic field (Gauss)
- */
-static inline float mavlink_msg_hil_sensor_get_ymag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  36);
-}
-
-/**
- * @brief Get field zmag from hil_sensor message
- *
- * @return Z Magnetic field (Gauss)
- */
-static inline float mavlink_msg_hil_sensor_get_zmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  40);
-}
-
-/**
- * @brief Get field abs_pressure from hil_sensor message
- *
- * @return Absolute pressure in millibar
- */
-static inline float mavlink_msg_hil_sensor_get_abs_pressure(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  44);
-}
-
-/**
- * @brief Get field diff_pressure from hil_sensor message
- *
- * @return Differential pressure (airspeed) in millibar
- */
-static inline float mavlink_msg_hil_sensor_get_diff_pressure(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  48);
-}
-
-/**
- * @brief Get field pressure_alt from hil_sensor message
- *
- * @return Altitude calculated from pressure
- */
-static inline float mavlink_msg_hil_sensor_get_pressure_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  52);
-}
-
-/**
- * @brief Get field temperature from hil_sensor message
- *
- * @return Temperature in degrees celsius
- */
-static inline float mavlink_msg_hil_sensor_get_temperature(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  56);
-}
-
-/**
- * @brief Get field fields_updated from hil_sensor message
- *
- * @return Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature
- */
-static inline uint32_t mavlink_msg_hil_sensor_get_fields_updated(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  60);
-}
-
-/**
- * @brief Decode a hil_sensor message into a struct
- *
- * @param msg The message to decode
- * @param hil_sensor C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hil_sensor_decode(const mavlink_message_t* msg, mavlink_hil_sensor_t* hil_sensor)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hil_sensor->time_usec = mavlink_msg_hil_sensor_get_time_usec(msg);
-	hil_sensor->xacc = mavlink_msg_hil_sensor_get_xacc(msg);
-	hil_sensor->yacc = mavlink_msg_hil_sensor_get_yacc(msg);
-	hil_sensor->zacc = mavlink_msg_hil_sensor_get_zacc(msg);
-	hil_sensor->xgyro = mavlink_msg_hil_sensor_get_xgyro(msg);
-	hil_sensor->ygyro = mavlink_msg_hil_sensor_get_ygyro(msg);
-	hil_sensor->zgyro = mavlink_msg_hil_sensor_get_zgyro(msg);
-	hil_sensor->xmag = mavlink_msg_hil_sensor_get_xmag(msg);
-	hil_sensor->ymag = mavlink_msg_hil_sensor_get_ymag(msg);
-	hil_sensor->zmag = mavlink_msg_hil_sensor_get_zmag(msg);
-	hil_sensor->abs_pressure = mavlink_msg_hil_sensor_get_abs_pressure(msg);
-	hil_sensor->diff_pressure = mavlink_msg_hil_sensor_get_diff_pressure(msg);
-	hil_sensor->pressure_alt = mavlink_msg_hil_sensor_get_pressure_alt(msg);
-	hil_sensor->temperature = mavlink_msg_hil_sensor_get_temperature(msg);
-	hil_sensor->fields_updated = mavlink_msg_hil_sensor_get_fields_updated(msg);
-#else
-	memcpy(hil_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_SENSOR_LEN);
-#endif
-}
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
deleted file mode 100644
index bcc857767..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h
+++ /dev/null
@@ -1,507 +0,0 @@
-// MESSAGE HIL_STATE PACKING
-
-#define MAVLINK_MSG_ID_HIL_STATE 90
-
-typedef struct __mavlink_hil_state_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- float roll; ///< Roll angle (rad)
- float pitch; ///< Pitch angle (rad)
- float yaw; ///< Yaw angle (rad)
- float rollspeed; ///< Body frame roll / phi angular speed (rad/s)
- float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s)
- float yawspeed; ///< Body frame yaw / psi angular speed (rad/s)
- int32_t lat; ///< Latitude, expressed as * 1E7
- int32_t lon; ///< Longitude, expressed as * 1E7
- int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters)
- int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100
- int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100
- int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100
- int16_t xacc; ///< X acceleration (mg)
- int16_t yacc; ///< Y acceleration (mg)
- int16_t zacc; ///< Z acceleration (mg)
-} mavlink_hil_state_t;
-
-#define MAVLINK_MSG_ID_HIL_STATE_LEN 56
-#define MAVLINK_MSG_ID_90_LEN 56
-
-#define MAVLINK_MSG_ID_HIL_STATE_CRC 183
-#define MAVLINK_MSG_ID_90_CRC 183
-
-
-
-#define MAVLINK_MESSAGE_INFO_HIL_STATE { \
-	"HIL_STATE", \
-	16, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_t, time_usec) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_state_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_state_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_state_t, yaw) }, \
-         { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_state_t, rollspeed) }, \
-         { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_t, pitchspeed) }, \
-         { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_t, yawspeed) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_hil_state_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_t, alt) }, \
-         { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_hil_state_t, vx) }, \
-         { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_hil_state_t, vy) }, \
-         { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_t, vz) }, \
-         { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_hil_state_t, zacc) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hil_state 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param rollspeed Body frame roll / phi angular speed (rad/s)
- * @param pitchspeed Body frame pitch / theta angular speed (rad/s)
- * @param yawspeed Body frame yaw / psi angular speed (rad/s)
- * @param lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HIL_STATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN);
-#else
-	mavlink_hil_state_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hil_state 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param rollspeed Body frame roll / phi angular speed (rad/s)
- * @param pitchspeed Body frame pitch / theta angular speed (rad/s)
- * @param yawspeed Body frame yaw / psi angular speed (rad/s)
- * @param lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HIL_STATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN);
-#else
-	mavlink_hil_state_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hil_state 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 hil_state C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state)
-{
-	return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc);
-}
-
-/**
- * @brief Encode a hil_state 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 hil_state C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state)
-{
-	return mavlink_msg_hil_state_pack_chan(system_id, component_id, chan, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc);
-}
-
-/**
- * @brief Send a hil_state message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param rollspeed Body frame roll / phi angular speed (rad/s)
- * @param pitchspeed Body frame pitch / theta angular speed (rad/s)
- * @param yawspeed Body frame yaw / psi angular speed (rad/s)
- * @param lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hil_state_send(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[MAVLINK_MSG_ID_HIL_STATE_LEN];
-	_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;
-	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
-
-// MESSAGE HIL_STATE UNPACKING
-
-
-/**
- * @brief Get field time_usec from hil_state message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_hil_state_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field roll from hil_state message
- *
- * @return Roll angle (rad)
- */
-static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field pitch from hil_state message
- *
- * @return Pitch angle (rad)
- */
-static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field yaw from hil_state message
- *
- * @return Yaw angle (rad)
- */
-static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field rollspeed from hil_state message
- *
- * @return Body frame roll / phi angular speed (rad/s)
- */
-static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field pitchspeed from hil_state message
- *
- * @return Body frame pitch / theta angular speed (rad/s)
- */
-static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field yawspeed from hil_state message
- *
- * @return Body frame yaw / psi angular speed (rad/s)
- */
-static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field lat from hil_state message
- *
- * @return Latitude, expressed as * 1E7
- */
-static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  32);
-}
-
-/**
- * @brief Get field lon from hil_state message
- *
- * @return Longitude, expressed as * 1E7
- */
-static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  36);
-}
-
-/**
- * @brief Get field alt from hil_state message
- *
- * @return Altitude in meters, expressed as * 1000 (millimeters)
- */
-static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  40);
-}
-
-/**
- * @brief Get field vx from hil_state message
- *
- * @return Ground X Speed (Latitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  44);
-}
-
-/**
- * @brief Get field vy from hil_state message
- *
- * @return Ground Y Speed (Longitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  46);
-}
-
-/**
- * @brief Get field vz from hil_state message
- *
- * @return Ground Z Speed (Altitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  48);
-}
-
-/**
- * @brief Get field xacc from hil_state message
- *
- * @return X acceleration (mg)
- */
-static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  50);
-}
-
-/**
- * @brief Get field yacc from hil_state message
- *
- * @return Y acceleration (mg)
- */
-static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  52);
-}
-
-/**
- * @brief Get field zacc from hil_state message
- *
- * @return Z acceleration (mg)
- */
-static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  54);
-}
-
-/**
- * @brief Decode a hil_state message into a struct
- *
- * @param msg The message to decode
- * @param hil_state C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hil_state->time_usec = mavlink_msg_hil_state_get_time_usec(msg);
-	hil_state->roll = mavlink_msg_hil_state_get_roll(msg);
-	hil_state->pitch = mavlink_msg_hil_state_get_pitch(msg);
-	hil_state->yaw = mavlink_msg_hil_state_get_yaw(msg);
-	hil_state->rollspeed = mavlink_msg_hil_state_get_rollspeed(msg);
-	hil_state->pitchspeed = mavlink_msg_hil_state_get_pitchspeed(msg);
-	hil_state->yawspeed = mavlink_msg_hil_state_get_yawspeed(msg);
-	hil_state->lat = mavlink_msg_hil_state_get_lat(msg);
-	hil_state->lon = mavlink_msg_hil_state_get_lon(msg);
-	hil_state->alt = mavlink_msg_hil_state_get_alt(msg);
-	hil_state->vx = mavlink_msg_hil_state_get_vx(msg);
-	hil_state->vy = mavlink_msg_hil_state_get_vy(msg);
-	hil_state->vz = mavlink_msg_hil_state_get_vz(msg);
-	hil_state->xacc = mavlink_msg_hil_state_get_xacc(msg);
-	hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg);
-	hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg);
-#else
-	memcpy(hil_state, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_STATE_LEN);
-#endif
-}
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
deleted file mode 100644
index 732176193..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h
+++ /dev/null
@@ -1,501 +0,0 @@
-// MESSAGE HIL_STATE_QUATERNION PACKING
-
-#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION 115
-
-typedef struct __mavlink_hil_state_quaternion_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- float attitude_quaternion[4]; ///< Vehicle attitude expressed as normalized quaternion
- float rollspeed; ///< Body frame roll / phi angular speed (rad/s)
- float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s)
- float yawspeed; ///< Body frame yaw / psi angular speed (rad/s)
- int32_t lat; ///< Latitude, expressed as * 1E7
- int32_t lon; ///< Longitude, expressed as * 1E7
- int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters)
- int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100
- int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100
- int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100
- uint16_t ind_airspeed; ///< Indicated airspeed, expressed as m/s * 100
- uint16_t true_airspeed; ///< True airspeed, expressed as m/s * 100
- int16_t xacc; ///< X acceleration (mg)
- int16_t yacc; ///< Y acceleration (mg)
- int16_t zacc; ///< Z acceleration (mg)
-} mavlink_hil_state_quaternion_t;
-
-#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN 64
-#define MAVLINK_MSG_ID_115_LEN 64
-
-#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC 4
-#define MAVLINK_MSG_ID_115_CRC 4
-
-#define MAVLINK_MSG_HIL_STATE_QUATERNION_FIELD_ATTITUDE_QUATERNION_LEN 4
-
-#define MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION { \
-	"HIL_STATE_QUATERNION", \
-	16, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_quaternion_t, time_usec) }, \
-         { "attitude_quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_hil_state_quaternion_t, attitude_quaternion) }, \
-         { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_quaternion_t, rollspeed) }, \
-         { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_quaternion_t, pitchspeed) }, \
-         { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_state_quaternion_t, yawspeed) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_quaternion_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_quaternion_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 44, offsetof(mavlink_hil_state_quaternion_t, alt) }, \
-         { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_quaternion_t, vx) }, \
-         { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_quaternion_t, vy) }, \
-         { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_quaternion_t, vz) }, \
-         { "ind_airspeed", NULL, MAVLINK_TYPE_UINT16_T, 0, 54, offsetof(mavlink_hil_state_quaternion_t, ind_airspeed) }, \
-         { "true_airspeed", NULL, MAVLINK_TYPE_UINT16_T, 0, 56, offsetof(mavlink_hil_state_quaternion_t, true_airspeed) }, \
-         { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_hil_state_quaternion_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_hil_state_quaternion_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 62, offsetof(mavlink_hil_state_quaternion_t, zacc) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hil_state_quaternion 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion
- * @param rollspeed Body frame roll / phi angular speed (rad/s)
- * @param pitchspeed Body frame pitch / theta angular speed (rad/s)
- * @param yawspeed Body frame yaw / psi angular speed (rad/s)
- * @param lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param ind_airspeed Indicated airspeed, expressed as m/s * 100
- * @param true_airspeed True airspeed, expressed as m/s * 100
- * @param xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
-#else
-	mavlink_hil_state_quaternion_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hil_state_quaternion 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion
- * @param rollspeed Body frame roll / phi angular speed (rad/s)
- * @param pitchspeed Body frame pitch / theta angular speed (rad/s)
- * @param yawspeed Body frame yaw / psi angular speed (rad/s)
- * @param lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param ind_airspeed Indicated airspeed, expressed as m/s * 100
- * @param true_airspeed True airspeed, expressed as m/s * 100
- * @param xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hil_state_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
-#else
-	mavlink_hil_state_quaternion_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hil_state_quaternion 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 hil_state_quaternion C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_state_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion)
-{
-	return mavlink_msg_hil_state_quaternion_pack(system_id, component_id, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc);
-}
-
-/**
- * @brief Encode a hil_state_quaternion 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 hil_state_quaternion C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion)
-{
-	return mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, chan, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc);
-}
-
-/**
- * @brief Send a hil_state_quaternion message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion
- * @param rollspeed Body frame roll / phi angular speed (rad/s)
- * @param pitchspeed Body frame pitch / theta angular speed (rad/s)
- * @param yawspeed Body frame yaw / psi angular speed (rad/s)
- * @param lat Latitude, expressed as * 1E7
- * @param lon Longitude, expressed as * 1E7
- * @param alt Altitude in meters, expressed as * 1000 (millimeters)
- * @param vx Ground X Speed (Latitude), expressed as m/s * 100
- * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
- * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
- * @param ind_airspeed Indicated airspeed, expressed as m/s * 100
- * @param true_airspeed True airspeed, expressed as m/s * 100
- * @param xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hil_state_quaternion_send(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[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN];
-	_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;
-	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
-
-// MESSAGE HIL_STATE_QUATERNION UNPACKING
-
-
-/**
- * @brief Get field time_usec from hil_state_quaternion message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_hil_state_quaternion_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field attitude_quaternion from hil_state_quaternion message
- *
- * @return Vehicle attitude expressed as normalized quaternion
- */
-static inline uint16_t mavlink_msg_hil_state_quaternion_get_attitude_quaternion(const mavlink_message_t* msg, float *attitude_quaternion)
-{
-	return _MAV_RETURN_float_array(msg, attitude_quaternion, 4,  8);
-}
-
-/**
- * @brief Get field rollspeed from hil_state_quaternion message
- *
- * @return Body frame roll / phi angular speed (rad/s)
- */
-static inline float mavlink_msg_hil_state_quaternion_get_rollspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field pitchspeed from hil_state_quaternion message
- *
- * @return Body frame pitch / theta angular speed (rad/s)
- */
-static inline float mavlink_msg_hil_state_quaternion_get_pitchspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field yawspeed from hil_state_quaternion message
- *
- * @return Body frame yaw / psi angular speed (rad/s)
- */
-static inline float mavlink_msg_hil_state_quaternion_get_yawspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field lat from hil_state_quaternion message
- *
- * @return Latitude, expressed as * 1E7
- */
-static inline int32_t mavlink_msg_hil_state_quaternion_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  36);
-}
-
-/**
- * @brief Get field lon from hil_state_quaternion message
- *
- * @return Longitude, expressed as * 1E7
- */
-static inline int32_t mavlink_msg_hil_state_quaternion_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  40);
-}
-
-/**
- * @brief Get field alt from hil_state_quaternion message
- *
- * @return Altitude in meters, expressed as * 1000 (millimeters)
- */
-static inline int32_t mavlink_msg_hil_state_quaternion_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  44);
-}
-
-/**
- * @brief Get field vx from hil_state_quaternion message
- *
- * @return Ground X Speed (Latitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_hil_state_quaternion_get_vx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  48);
-}
-
-/**
- * @brief Get field vy from hil_state_quaternion message
- *
- * @return Ground Y Speed (Longitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_hil_state_quaternion_get_vy(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  50);
-}
-
-/**
- * @brief Get field vz from hil_state_quaternion message
- *
- * @return Ground Z Speed (Altitude), expressed as m/s * 100
- */
-static inline int16_t mavlink_msg_hil_state_quaternion_get_vz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  52);
-}
-
-/**
- * @brief Get field ind_airspeed from hil_state_quaternion message
- *
- * @return Indicated airspeed, expressed as m/s * 100
- */
-static inline uint16_t mavlink_msg_hil_state_quaternion_get_ind_airspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  54);
-}
-
-/**
- * @brief Get field true_airspeed from hil_state_quaternion message
- *
- * @return True airspeed, expressed as m/s * 100
- */
-static inline uint16_t mavlink_msg_hil_state_quaternion_get_true_airspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  56);
-}
-
-/**
- * @brief Get field xacc from hil_state_quaternion message
- *
- * @return X acceleration (mg)
- */
-static inline int16_t mavlink_msg_hil_state_quaternion_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  58);
-}
-
-/**
- * @brief Get field yacc from hil_state_quaternion message
- *
- * @return Y acceleration (mg)
- */
-static inline int16_t mavlink_msg_hil_state_quaternion_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  60);
-}
-
-/**
- * @brief Get field zacc from hil_state_quaternion message
- *
- * @return Z acceleration (mg)
- */
-static inline int16_t mavlink_msg_hil_state_quaternion_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  62);
-}
-
-/**
- * @brief Decode a hil_state_quaternion message into a struct
- *
- * @param msg The message to decode
- * @param hil_state_quaternion C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hil_state_quaternion_decode(const mavlink_message_t* msg, mavlink_hil_state_quaternion_t* hil_state_quaternion)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hil_state_quaternion->time_usec = mavlink_msg_hil_state_quaternion_get_time_usec(msg);
-	mavlink_msg_hil_state_quaternion_get_attitude_quaternion(msg, hil_state_quaternion->attitude_quaternion);
-	hil_state_quaternion->rollspeed = mavlink_msg_hil_state_quaternion_get_rollspeed(msg);
-	hil_state_quaternion->pitchspeed = mavlink_msg_hil_state_quaternion_get_pitchspeed(msg);
-	hil_state_quaternion->yawspeed = mavlink_msg_hil_state_quaternion_get_yawspeed(msg);
-	hil_state_quaternion->lat = mavlink_msg_hil_state_quaternion_get_lat(msg);
-	hil_state_quaternion->lon = mavlink_msg_hil_state_quaternion_get_lon(msg);
-	hil_state_quaternion->alt = mavlink_msg_hil_state_quaternion_get_alt(msg);
-	hil_state_quaternion->vx = mavlink_msg_hil_state_quaternion_get_vx(msg);
-	hil_state_quaternion->vy = mavlink_msg_hil_state_quaternion_get_vy(msg);
-	hil_state_quaternion->vz = mavlink_msg_hil_state_quaternion_get_vz(msg);
-	hil_state_quaternion->ind_airspeed = mavlink_msg_hil_state_quaternion_get_ind_airspeed(msg);
-	hil_state_quaternion->true_airspeed = mavlink_msg_hil_state_quaternion_get_true_airspeed(msg);
-	hil_state_quaternion->xacc = mavlink_msg_hil_state_quaternion_get_xacc(msg);
-	hil_state_quaternion->yacc = mavlink_msg_hil_state_quaternion_get_yacc(msg);
-	hil_state_quaternion->zacc = mavlink_msg_hil_state_quaternion_get_zacc(msg);
-#else
-	memcpy(hil_state_quaternion, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
-#endif
-}
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
deleted file mode 100644
index a0b72c0e1..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE LOCAL_POSITION_NED PACKING
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_NED 32
-
-typedef struct __mavlink_local_position_ned_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float x; ///< X Position
- float y; ///< Y Position
- float z; ///< Z Position
- float vx; ///< X Speed
- float vy; ///< Y Speed
- float vz; ///< Z Speed
-} mavlink_local_position_ned_t;
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN 28
-#define MAVLINK_MSG_ID_32_LEN 28
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC 185
-#define MAVLINK_MSG_ID_32_CRC 185
-
-
-
-#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED { \
-	"LOCAL_POSITION_NED", \
-	7, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_local_position_ned_t, time_boot_ms) }, \
-         { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_ned_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_ned_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_t, z) }, \
-         { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_t, vx) }, \
-         { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_t, vy) }, \
-         { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_t, vz) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a local_position_ned 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 x X Position
- * @param y Y Position
- * @param z Z Position
- * @param vx X Speed
- * @param vy Y Speed
- * @param vz Z Speed
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
-#else
-	mavlink_local_position_ned_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
-#endif
-}
-
-/**
- * @brief Pack a local_position_ned 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 x X Position
- * @param y Y Position
- * @param z Z Position
- * @param vx X Speed
- * @param vy Y Speed
- * @param vz Z Speed
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
-#else
-	mavlink_local_position_ned_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
-#endif
-}
-
-/**
- * @brief Encode a local_position_ned 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 local_position_ned C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_local_position_ned_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned)
-{
-	return mavlink_msg_local_position_ned_pack(system_id, component_id, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz);
-}
-
-/**
- * @brief Encode a local_position_ned 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 local_position_ned C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_local_position_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned)
-{
-	return mavlink_msg_local_position_ned_pack_chan(system_id, component_id, chan, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz);
-}
-
-/**
- * @brief Send a local_position_ned message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param x X Position
- * @param y Y Position
- * @param z Z Position
- * @param vx X Speed
- * @param vy Y Speed
- * @param vz Z Speed
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_local_position_ned_send(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[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN];
-	_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;
-	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
-
-// MESSAGE LOCAL_POSITION_NED UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from local_position_ned message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_local_position_ned_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field x from local_position_ned message
- *
- * @return X Position
- */
-static inline float mavlink_msg_local_position_ned_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field y from local_position_ned message
- *
- * @return Y Position
- */
-static inline float mavlink_msg_local_position_ned_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field z from local_position_ned message
- *
- * @return Z Position
- */
-static inline float mavlink_msg_local_position_ned_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field vx from local_position_ned message
- *
- * @return X Speed
- */
-static inline float mavlink_msg_local_position_ned_get_vx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field vy from local_position_ned message
- *
- * @return Y Speed
- */
-static inline float mavlink_msg_local_position_ned_get_vy(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field vz from local_position_ned message
- *
- * @return Z Speed
- */
-static inline float mavlink_msg_local_position_ned_get_vz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a local_position_ned message into a struct
- *
- * @param msg The message to decode
- * @param local_position_ned C-struct to decode the message contents into
- */
-static inline void mavlink_msg_local_position_ned_decode(const mavlink_message_t* msg, mavlink_local_position_ned_t* local_position_ned)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	local_position_ned->time_boot_ms = mavlink_msg_local_position_ned_get_time_boot_ms(msg);
-	local_position_ned->x = mavlink_msg_local_position_ned_get_x(msg);
-	local_position_ned->y = mavlink_msg_local_position_ned_get_y(msg);
-	local_position_ned->z = mavlink_msg_local_position_ned_get_z(msg);
-	local_position_ned->vx = mavlink_msg_local_position_ned_get_vx(msg);
-	local_position_ned->vy = mavlink_msg_local_position_ned_get_vy(msg);
-	local_position_ned->vz = mavlink_msg_local_position_ned_get_vz(msg);
-#else
-	memcpy(local_position_ned, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
-#endif
-}
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
deleted file mode 100644
index 8c4686202..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET PACKING
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET 89
-
-typedef struct __mavlink_local_position_ned_system_global_offset_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float x; ///< X Position
- float y; ///< Y Position
- float z; ///< Z Position
- float roll; ///< Roll
- float pitch; ///< Pitch
- float yaw; ///< Yaw
-} mavlink_local_position_ned_system_global_offset_t;
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN 28
-#define MAVLINK_MSG_ID_89_LEN 28
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC 231
-#define MAVLINK_MSG_ID_89_CRC 231
-
-
-
-#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET { \
-	"LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", \
-	7, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_local_position_ned_system_global_offset_t, time_boot_ms) }, \
-         { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_ned_system_global_offset_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_ned_system_global_offset_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_system_global_offset_t, z) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_system_global_offset_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_system_global_offset_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_system_global_offset_t, yaw) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a local_position_ned_system_global_offset 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 x X Position
- * @param y Y Position
- * @param z Z Position
- * @param roll Roll
- * @param pitch Pitch
- * @param yaw Yaw
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
-#else
-	mavlink_local_position_ned_system_global_offset_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
-#endif
-}
-
-/**
- * @brief Pack a local_position_ned_system_global_offset 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 x X Position
- * @param y Y Position
- * @param z Z Position
- * @param roll Roll
- * @param pitch Pitch
- * @param yaw Yaw
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
-#else
-	mavlink_local_position_ned_system_global_offset_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
-#endif
-}
-
-/**
- * @brief Encode a local_position_ned_system_global_offset 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 local_position_ned_system_global_offset C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset)
-{
-	return mavlink_msg_local_position_ned_system_global_offset_pack(system_id, component_id, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw);
-}
-
-/**
- * @brief Encode a local_position_ned_system_global_offset 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 local_position_ned_system_global_offset C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset)
-{
-	return mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, chan, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw);
-}
-
-/**
- * @brief Send a local_position_ned_system_global_offset message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param x X Position
- * @param y Y Position
- * @param z Z Position
- * @param roll Roll
- * @param pitch Pitch
- * @param yaw Yaw
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_local_position_ned_system_global_offset_send(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[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN];
-	_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;
-	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
-
-// MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from local_position_ned_system_global_offset message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_local_position_ned_system_global_offset_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field x from local_position_ned_system_global_offset message
- *
- * @return X Position
- */
-static inline float mavlink_msg_local_position_ned_system_global_offset_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field y from local_position_ned_system_global_offset message
- *
- * @return Y Position
- */
-static inline float mavlink_msg_local_position_ned_system_global_offset_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field z from local_position_ned_system_global_offset message
- *
- * @return Z Position
- */
-static inline float mavlink_msg_local_position_ned_system_global_offset_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field roll from local_position_ned_system_global_offset message
- *
- * @return Roll
- */
-static inline float mavlink_msg_local_position_ned_system_global_offset_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field pitch from local_position_ned_system_global_offset message
- *
- * @return Pitch
- */
-static inline float mavlink_msg_local_position_ned_system_global_offset_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field yaw from local_position_ned_system_global_offset message
- *
- * @return Yaw
- */
-static inline float mavlink_msg_local_position_ned_system_global_offset_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a local_position_ned_system_global_offset message into a struct
- *
- * @param msg The message to decode
- * @param local_position_ned_system_global_offset C-struct to decode the message contents into
- */
-static inline void mavlink_msg_local_position_ned_system_global_offset_decode(const mavlink_message_t* msg, mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	local_position_ned_system_global_offset->time_boot_ms = mavlink_msg_local_position_ned_system_global_offset_get_time_boot_ms(msg);
-	local_position_ned_system_global_offset->x = mavlink_msg_local_position_ned_system_global_offset_get_x(msg);
-	local_position_ned_system_global_offset->y = mavlink_msg_local_position_ned_system_global_offset_get_y(msg);
-	local_position_ned_system_global_offset->z = mavlink_msg_local_position_ned_system_global_offset_get_z(msg);
-	local_position_ned_system_global_offset->roll = mavlink_msg_local_position_ned_system_global_offset_get_roll(msg);
-	local_position_ned_system_global_offset->pitch = mavlink_msg_local_position_ned_system_global_offset_get_pitch(msg);
-	local_position_ned_system_global_offset->yaw = mavlink_msg_local_position_ned_system_global_offset_get_yaw(msg);
-#else
-	memcpy(local_position_ned_system_global_offset, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
-#endif
-}
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
deleted file mode 100644
index 1794815f8..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE LOCAL_POSITION_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51
-
-typedef struct __mavlink_local_position_setpoint_t
-{
- float x; ///< x position
- float y; ///< y position
- float z; ///< z position
- float yaw; ///< Desired yaw angle
- uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
-} mavlink_local_position_setpoint_t;
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 17
-#define MAVLINK_MSG_ID_51_LEN 17
-
-#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC 223
-#define MAVLINK_MSG_ID_51_CRC 223
-
-
-
-#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT { \
-	"LOCAL_POSITION_SETPOINT", \
-	5, \
-	{  { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_local_position_setpoint_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_setpoint_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_setpoint_t, z) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_setpoint_t, yaw) }, \
-         { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_local_position_setpoint_t, coordinate_frame) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a local_position_setpoint 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 coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw Desired yaw angle
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t coordinate_frame, float x, float y, float z, float yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
-#else
-	mavlink_local_position_setpoint_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.yaw = yaw;
-	packet.coordinate_frame = coordinate_frame;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a local_position_setpoint 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 coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw Desired yaw angle
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t coordinate_frame,float x,float y,float z,float yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
-#else
-	mavlink_local_position_setpoint_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.yaw = yaw;
-	packet.coordinate_frame = coordinate_frame;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a local_position_setpoint 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 local_position_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint)
-{
-	return mavlink_msg_local_position_setpoint_pack(system_id, component_id, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw);
-}
-
-/**
- * @brief Encode a local_position_setpoint 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 local_position_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint)
-{
-	return mavlink_msg_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw);
-}
-
-/**
- * @brief Send a local_position_setpoint message
- * @param chan MAVLink channel to send the message
- *
- * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw Desired yaw angle
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_local_position_setpoint_send(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[MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN];
-	_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;
-	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
-
-// MESSAGE LOCAL_POSITION_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field coordinate_frame from local_position_setpoint message
- *
- * @return Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
- */
-static inline uint8_t mavlink_msg_local_position_setpoint_get_coordinate_frame(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field x from local_position_setpoint message
- *
- * @return x position
- */
-static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field y from local_position_setpoint message
- *
- * @return y position
- */
-static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field z from local_position_setpoint message
- *
- * @return z position
- */
-static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw from local_position_setpoint message
- *
- * @return Desired yaw angle
- */
-static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Decode a local_position_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param local_position_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_t* local_position_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	local_position_setpoint->x = mavlink_msg_local_position_setpoint_get_x(msg);
-	local_position_setpoint->y = mavlink_msg_local_position_setpoint_get_y(msg);
-	local_position_setpoint->z = mavlink_msg_local_position_setpoint_get_z(msg);
-	local_position_setpoint->yaw = mavlink_msg_local_position_setpoint_get_yaw(msg);
-	local_position_setpoint->coordinate_frame = mavlink_msg_local_position_setpoint_get_coordinate_frame(msg);
-#else
-	memcpy(local_position_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-}
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
deleted file mode 100644
index 1cf5d15e4..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h
+++ /dev/null
@@ -1,237 +0,0 @@
-// MESSAGE LOG_DATA PACKING
-
-#define MAVLINK_MSG_ID_LOG_DATA 120
-
-typedef struct __mavlink_log_data_t
-{
- uint32_t ofs; ///< Offset into the log
- uint16_t id; ///< Log id (from LOG_ENTRY reply)
- uint8_t count; ///< Number of bytes (zero for end of log)
- uint8_t data[90]; ///< log data
-} mavlink_log_data_t;
-
-#define MAVLINK_MSG_ID_LOG_DATA_LEN 97
-#define MAVLINK_MSG_ID_120_LEN 97
-
-#define MAVLINK_MSG_ID_LOG_DATA_CRC 134
-#define MAVLINK_MSG_ID_120_CRC 134
-
-#define MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN 90
-
-#define MAVLINK_MESSAGE_INFO_LOG_DATA { \
-	"LOG_DATA", \
-	4, \
-	{  { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_data_t, ofs) }, \
-         { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_log_data_t, id) }, \
-         { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_log_data_t, count) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 90, 7, offsetof(mavlink_log_data_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a log_data 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 id Log id (from LOG_ENTRY reply)
- * @param ofs Offset into the log
- * @param count Number of bytes (zero for end of log)
- * @param data log data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_DATA_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN);
-#else
-	mavlink_log_data_t packet;
-	packet.ofs = ofs;
-	packet.id = id;
-	packet.count = count;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_DATA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN);
-#endif
-}
-
-/**
- * @brief Pack a log_data 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 id Log id (from LOG_ENTRY reply)
- * @param ofs Offset into the log
- * @param count Number of bytes (zero for end of log)
- * @param data log data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t id,uint32_t ofs,uint8_t count,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_DATA_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN);
-#else
-	mavlink_log_data_t packet;
-	packet.ofs = ofs;
-	packet.id = id;
-	packet.count = count;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_DATA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN);
-#endif
-}
-
-/**
- * @brief Encode a log_data 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 log_data C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_data_t* log_data)
-{
-	return mavlink_msg_log_data_pack(system_id, component_id, msg, log_data->id, log_data->ofs, log_data->count, log_data->data);
-}
-
-/**
- * @brief Encode a log_data 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 log_data C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_data_t* log_data)
-{
-	return mavlink_msg_log_data_pack_chan(system_id, component_id, chan, msg, log_data->id, log_data->ofs, log_data->count, log_data->data);
-}
-
-/**
- * @brief Send a log_data message
- * @param chan MAVLink channel to send the message
- *
- * @param id Log id (from LOG_ENTRY reply)
- * @param ofs Offset into the log
- * @param count Number of bytes (zero for end of log)
- * @param data log data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_log_data_send(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[MAVLINK_MSG_ID_LOG_DATA_LEN];
-	_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;
-	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
-
-// MESSAGE LOG_DATA UNPACKING
-
-
-/**
- * @brief Get field id from log_data message
- *
- * @return Log id (from LOG_ENTRY reply)
- */
-static inline uint16_t mavlink_msg_log_data_get_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field ofs from log_data message
- *
- * @return Offset into the log
- */
-static inline uint32_t mavlink_msg_log_data_get_ofs(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field count from log_data message
- *
- * @return Number of bytes (zero for end of log)
- */
-static inline uint8_t mavlink_msg_log_data_get_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field data from log_data message
- *
- * @return log data
- */
-static inline uint16_t mavlink_msg_log_data_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 90,  7);
-}
-
-/**
- * @brief Decode a log_data message into a struct
- *
- * @param msg The message to decode
- * @param log_data C-struct to decode the message contents into
- */
-static inline void mavlink_msg_log_data_decode(const mavlink_message_t* msg, mavlink_log_data_t* log_data)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	log_data->ofs = mavlink_msg_log_data_get_ofs(msg);
-	log_data->id = mavlink_msg_log_data_get_id(msg);
-	log_data->count = mavlink_msg_log_data_get_count(msg);
-	mavlink_msg_log_data_get_data(msg, log_data->data);
-#else
-	memcpy(log_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_DATA_LEN);
-#endif
-}
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
deleted file mode 100644
index 681d8f07c..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE LOG_ENTRY PACKING
-
-#define MAVLINK_MSG_ID_LOG_ENTRY 118
-
-typedef struct __mavlink_log_entry_t
-{
- uint32_t time_utc; ///< UTC timestamp of log in seconds since 1970, or 0 if not available
- uint32_t size; ///< Size of the log (may be approximate) in bytes
- uint16_t id; ///< Log id
- uint16_t num_logs; ///< Total number of logs
- uint16_t last_log_num; ///< High log number
-} mavlink_log_entry_t;
-
-#define MAVLINK_MSG_ID_LOG_ENTRY_LEN 14
-#define MAVLINK_MSG_ID_118_LEN 14
-
-#define MAVLINK_MSG_ID_LOG_ENTRY_CRC 56
-#define MAVLINK_MSG_ID_118_CRC 56
-
-
-
-#define MAVLINK_MESSAGE_INFO_LOG_ENTRY { \
-	"LOG_ENTRY", \
-	5, \
-	{  { "time_utc", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_entry_t, time_utc) }, \
-         { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_entry_t, size) }, \
-         { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_entry_t, id) }, \
-         { "num_logs", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_log_entry_t, num_logs) }, \
-         { "last_log_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_log_entry_t, last_log_num) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a log_entry 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 id Log id
- * @param num_logs Total number of logs
- * @param last_log_num High log number
- * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available
- * @param size Size of the log (may be approximate) in bytes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_entry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_LOG_ENTRY_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
-#else
-	mavlink_log_entry_t packet;
-	packet.time_utc = time_utc;
-	packet.size = size;
-	packet.id = id;
-	packet.num_logs = num_logs;
-	packet.last_log_num = last_log_num;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
-#endif
-}
-
-/**
- * @brief Pack a log_entry 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 id Log id
- * @param num_logs Total number of logs
- * @param last_log_num High log number
- * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available
- * @param size Size of the log (may be approximate) in bytes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_entry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_LOG_ENTRY_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
-#else
-	mavlink_log_entry_t packet;
-	packet.time_utc = time_utc;
-	packet.size = size;
-	packet.id = id;
-	packet.num_logs = num_logs;
-	packet.last_log_num = last_log_num;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
-#endif
-}
-
-/**
- * @brief Encode a log_entry 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 log_entry C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_entry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry)
-{
-	return mavlink_msg_log_entry_pack(system_id, component_id, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size);
-}
-
-/**
- * @brief Encode a log_entry 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 log_entry C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_entry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry)
-{
-	return mavlink_msg_log_entry_pack_chan(system_id, component_id, chan, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size);
-}
-
-/**
- * @brief Send a log_entry message
- * @param chan MAVLink channel to send the message
- *
- * @param id Log id
- * @param num_logs Total number of logs
- * @param last_log_num High log number
- * @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available
- * @param size Size of the log (may be approximate) in bytes
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_log_entry_send(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[MAVLINK_MSG_ID_LOG_ENTRY_LEN];
-	_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;
-	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
-
-// MESSAGE LOG_ENTRY UNPACKING
-
-
-/**
- * @brief Get field id from log_entry message
- *
- * @return Log id
- */
-static inline uint16_t mavlink_msg_log_entry_get_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field num_logs from log_entry message
- *
- * @return Total number of logs
- */
-static inline uint16_t mavlink_msg_log_entry_get_num_logs(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Get field last_log_num from log_entry message
- *
- * @return High log number
- */
-static inline uint16_t mavlink_msg_log_entry_get_last_log_num(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field time_utc from log_entry message
- *
- * @return UTC timestamp of log in seconds since 1970, or 0 if not available
- */
-static inline uint32_t mavlink_msg_log_entry_get_time_utc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field size from log_entry message
- *
- * @return Size of the log (may be approximate) in bytes
- */
-static inline uint32_t mavlink_msg_log_entry_get_size(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  4);
-}
-
-/**
- * @brief Decode a log_entry message into a struct
- *
- * @param msg The message to decode
- * @param log_entry C-struct to decode the message contents into
- */
-static inline void mavlink_msg_log_entry_decode(const mavlink_message_t* msg, mavlink_log_entry_t* log_entry)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	log_entry->time_utc = mavlink_msg_log_entry_get_time_utc(msg);
-	log_entry->size = mavlink_msg_log_entry_get_size(msg);
-	log_entry->id = mavlink_msg_log_entry_get_id(msg);
-	log_entry->num_logs = mavlink_msg_log_entry_get_num_logs(msg);
-	log_entry->last_log_num = mavlink_msg_log_entry_get_last_log_num(msg);
-#else
-	memcpy(log_entry, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ENTRY_LEN);
-#endif
-}
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
deleted file mode 100644
index feafeaf16..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE LOG_ERASE PACKING
-
-#define MAVLINK_MSG_ID_LOG_ERASE 121
-
-typedef struct __mavlink_log_erase_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_log_erase_t;
-
-#define MAVLINK_MSG_ID_LOG_ERASE_LEN 2
-#define MAVLINK_MSG_ID_121_LEN 2
-
-#define MAVLINK_MSG_ID_LOG_ERASE_CRC 237
-#define MAVLINK_MSG_ID_121_CRC 237
-
-
-
-#define MAVLINK_MESSAGE_INFO_LOG_ERASE { \
-	"LOG_ERASE", \
-	2, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_erase_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_erase_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a log_erase 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_erase_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN);
-#else
-	mavlink_log_erase_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_ERASE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a log_erase 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_erase_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN);
-#else
-	mavlink_log_erase_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_ERASE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a log_erase 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 log_erase C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_erase_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase)
-{
-	return mavlink_msg_log_erase_pack(system_id, component_id, msg, log_erase->target_system, log_erase->target_component);
-}
-
-/**
- * @brief Encode a log_erase 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 log_erase C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_erase_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase)
-{
-	return mavlink_msg_log_erase_pack_chan(system_id, component_id, chan, msg, log_erase->target_system, log_erase->target_component);
-}
-
-/**
- * @brief Send a log_erase message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN];
-	_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;
-	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
-
-// MESSAGE LOG_ERASE UNPACKING
-
-
-/**
- * @brief Get field target_system from log_erase message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_log_erase_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from log_erase message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_log_erase_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Decode a log_erase message into a struct
- *
- * @param msg The message to decode
- * @param log_erase C-struct to decode the message contents into
- */
-static inline void mavlink_msg_log_erase_decode(const mavlink_message_t* msg, mavlink_log_erase_t* log_erase)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	log_erase->target_system = mavlink_msg_log_erase_get_target_system(msg);
-	log_erase->target_component = mavlink_msg_log_erase_get_target_component(msg);
-#else
-	memcpy(log_erase, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ERASE_LEN);
-#endif
-}
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
deleted file mode 100644
index 5be9eea47..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE LOG_REQUEST_DATA PACKING
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_DATA 119
-
-typedef struct __mavlink_log_request_data_t
-{
- uint32_t ofs; ///< Offset into the log
- uint32_t count; ///< Number of bytes
- uint16_t id; ///< Log id (from LOG_ENTRY reply)
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_log_request_data_t;
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN 12
-#define MAVLINK_MSG_ID_119_LEN 12
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC 116
-#define MAVLINK_MSG_ID_119_CRC 116
-
-
-
-#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA { \
-	"LOG_REQUEST_DATA", \
-	5, \
-	{  { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_request_data_t, ofs) }, \
-         { "count", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_request_data_t, count) }, \
-         { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_request_data_t, id) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_log_request_data_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_log_request_data_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a log_request_data 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 target_system System ID
- * @param target_component Component ID
- * @param id Log id (from LOG_ENTRY reply)
- * @param ofs Offset into the log
- * @param count Number of bytes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_request_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
-#else
-	mavlink_log_request_data_t packet;
-	packet.ofs = ofs;
-	packet.count = count;
-	packet.id = id;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
-#endif
-}
-
-/**
- * @brief Pack a log_request_data 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 target_system System ID
- * @param target_component Component ID
- * @param id Log id (from LOG_ENTRY reply)
- * @param ofs Offset into the log
- * @param count Number of bytes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_request_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
-#else
-	mavlink_log_request_data_t packet;
-	packet.ofs = ofs;
-	packet.count = count;
-	packet.id = id;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
-#endif
-}
-
-/**
- * @brief Encode a log_request_data 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 log_request_data C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_request_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data)
-{
-	return mavlink_msg_log_request_data_pack(system_id, component_id, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count);
-}
-
-/**
- * @brief Encode a log_request_data 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 log_request_data C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_request_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data)
-{
-	return mavlink_msg_log_request_data_pack_chan(system_id, component_id, chan, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count);
-}
-
-/**
- * @brief Send a log_request_data message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param id Log id (from LOG_ENTRY reply)
- * @param ofs Offset into the log
- * @param count Number of bytes
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_log_request_data_send(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[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN];
-	_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;
-	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
-
-// MESSAGE LOG_REQUEST_DATA UNPACKING
-
-
-/**
- * @brief Get field target_system from log_request_data message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_log_request_data_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field target_component from log_request_data message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_log_request_data_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field id from log_request_data message
- *
- * @return Log id (from LOG_ENTRY reply)
- */
-static inline uint16_t mavlink_msg_log_request_data_get_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field ofs from log_request_data message
- *
- * @return Offset into the log
- */
-static inline uint32_t mavlink_msg_log_request_data_get_ofs(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field count from log_request_data message
- *
- * @return Number of bytes
- */
-static inline uint32_t mavlink_msg_log_request_data_get_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  4);
-}
-
-/**
- * @brief Decode a log_request_data message into a struct
- *
- * @param msg The message to decode
- * @param log_request_data C-struct to decode the message contents into
- */
-static inline void mavlink_msg_log_request_data_decode(const mavlink_message_t* msg, mavlink_log_request_data_t* log_request_data)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	log_request_data->ofs = mavlink_msg_log_request_data_get_ofs(msg);
-	log_request_data->count = mavlink_msg_log_request_data_get_count(msg);
-	log_request_data->id = mavlink_msg_log_request_data_get_id(msg);
-	log_request_data->target_system = mavlink_msg_log_request_data_get_target_system(msg);
-	log_request_data->target_component = mavlink_msg_log_request_data_get_target_component(msg);
-#else
-	memcpy(log_request_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
-#endif
-}
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
deleted file mode 100644
index 48a5a03b4..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE LOG_REQUEST_END PACKING
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_END 122
-
-typedef struct __mavlink_log_request_end_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_log_request_end_t;
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_END_LEN 2
-#define MAVLINK_MSG_ID_122_LEN 2
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_END_CRC 203
-#define MAVLINK_MSG_ID_122_CRC 203
-
-
-
-#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_END { \
-	"LOG_REQUEST_END", \
-	2, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_request_end_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_request_end_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a log_request_end 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_request_end_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
-#else
-	mavlink_log_request_end_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
-#endif
-}
-
-/**
- * @brief Pack a log_request_end 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_request_end_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
-#else
-	mavlink_log_request_end_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
-#endif
-}
-
-/**
- * @brief Encode a log_request_end 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 log_request_end C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_request_end_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end)
-{
-	return mavlink_msg_log_request_end_pack(system_id, component_id, msg, log_request_end->target_system, log_request_end->target_component);
-}
-
-/**
- * @brief Encode a log_request_end 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 log_request_end C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_request_end_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end)
-{
-	return mavlink_msg_log_request_end_pack_chan(system_id, component_id, chan, msg, log_request_end->target_system, log_request_end->target_component);
-}
-
-/**
- * @brief Send a log_request_end message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN];
-	_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;
-	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
-
-// MESSAGE LOG_REQUEST_END UNPACKING
-
-
-/**
- * @brief Get field target_system from log_request_end message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_log_request_end_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from log_request_end message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_log_request_end_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Decode a log_request_end message into a struct
- *
- * @param msg The message to decode
- * @param log_request_end C-struct to decode the message contents into
- */
-static inline void mavlink_msg_log_request_end_decode(const mavlink_message_t* msg, mavlink_log_request_end_t* log_request_end)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	log_request_end->target_system = mavlink_msg_log_request_end_get_target_system(msg);
-	log_request_end->target_component = mavlink_msg_log_request_end_get_target_component(msg);
-#else
-	memcpy(log_request_end, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
-#endif
-}
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
deleted file mode 100644
index 7a5b25c17..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE LOG_REQUEST_LIST PACKING
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_LIST 117
-
-typedef struct __mavlink_log_request_list_t
-{
- uint16_t start; ///< First log id (0 for first available)
- uint16_t end; ///< Last log id (0xffff for last available)
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_log_request_list_t;
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN 6
-#define MAVLINK_MSG_ID_117_LEN 6
-
-#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC 128
-#define MAVLINK_MSG_ID_117_CRC 128
-
-
-
-#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST { \
-	"LOG_REQUEST_LIST", \
-	4, \
-	{  { "start", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_log_request_list_t, start) }, \
-         { "end", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_log_request_list_t, end) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_log_request_list_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_log_request_list_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a log_request_list 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 target_system System ID
- * @param target_component Component ID
- * @param start First log id (0 for first available)
- * @param end Last log id (0xffff for last available)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
-#else
-	mavlink_log_request_list_t packet;
-	packet.start = start;
-	packet.end = end;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a log_request_list 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 target_system System ID
- * @param target_component Component ID
- * @param start First log id (0 for first available)
- * @param end Last log id (0xffff for last available)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_log_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint16_t start,uint16_t end)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
-#else
-	mavlink_log_request_list_t packet;
-	packet.start = start;
-	packet.end = end;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a log_request_list 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 log_request_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list)
-{
-	return mavlink_msg_log_request_list_pack(system_id, component_id, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end);
-}
-
-/**
- * @brief Encode a log_request_list 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 log_request_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_log_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list)
-{
-	return mavlink_msg_log_request_list_pack_chan(system_id, component_id, chan, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end);
-}
-
-/**
- * @brief Send a log_request_list message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param start First log id (0 for first available)
- * @param end Last log id (0xffff for last available)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_log_request_list_send(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[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN];
-	_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;
-	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
-
-// MESSAGE LOG_REQUEST_LIST UNPACKING
-
-
-/**
- * @brief Get field target_system from log_request_list message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_log_request_list_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field target_component from log_request_list message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_log_request_list_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field start from log_request_list message
- *
- * @return First log id (0 for first available)
- */
-static inline uint16_t mavlink_msg_log_request_list_get_start(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field end from log_request_list message
- *
- * @return Last log id (0xffff for last available)
- */
-static inline uint16_t mavlink_msg_log_request_list_get_end(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a log_request_list message into a struct
- *
- * @param msg The message to decode
- * @param log_request_list C-struct to decode the message contents into
- */
-static inline void mavlink_msg_log_request_list_decode(const mavlink_message_t* msg, mavlink_log_request_list_t* log_request_list)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	log_request_list->start = mavlink_msg_log_request_list_get_start(msg);
-	log_request_list->end = mavlink_msg_log_request_list_get_end(msg);
-	log_request_list->target_system = mavlink_msg_log_request_list_get_target_system(msg);
-	log_request_list->target_component = mavlink_msg_log_request_list_get_target_component(msg);
-#else
-	memcpy(log_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
-#endif
-}
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
deleted file mode 100644
index 6b6e9e148..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE MANUAL_CONTROL PACKING
-
-#define MAVLINK_MSG_ID_MANUAL_CONTROL 69
-
-typedef struct __mavlink_manual_control_t
-{
- int16_t x; ///< X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle.
- int16_t y; ///< Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle.
- int16_t z; ///< Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle.
- int16_t r; ///< R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle.
- uint16_t buttons; ///< A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.
- uint8_t target; ///< The system to be controlled.
-} mavlink_manual_control_t;
-
-#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 11
-#define MAVLINK_MSG_ID_69_LEN 11
-
-#define MAVLINK_MSG_ID_MANUAL_CONTROL_CRC 243
-#define MAVLINK_MSG_ID_69_CRC 243
-
-
-
-#define MAVLINK_MESSAGE_INFO_MANUAL_CONTROL { \
-	"MANUAL_CONTROL", \
-	6, \
-	{  { "x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_manual_control_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_manual_control_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_manual_control_t, z) }, \
-         { "r", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_manual_control_t, r) }, \
-         { "buttons", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_manual_control_t, buttons) }, \
-         { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_manual_control_t, target) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a manual_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 target The system to be controlled.
- * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle.
- * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle.
- * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle.
- * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle.
- * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
-#else
-	mavlink_manual_control_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.r = r;
-	packet.buttons = buttons;
-	packet.target = target;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a manual_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 target The system to be controlled.
- * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle.
- * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle.
- * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle.
- * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle.
- * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
-#else
-	mavlink_manual_control_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.r = r;
-	packet.buttons = buttons;
-	packet.target = target;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a manual_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 manual_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control)
-{
-	return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons);
-}
-
-/**
- * @brief Encode a manual_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 manual_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control)
-{
-	return mavlink_msg_manual_control_pack_chan(system_id, component_id, chan, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons);
-}
-
-/**
- * @brief Send a manual_control message
- * @param chan MAVLink channel to send the message
- *
- * @param target The system to be controlled.
- * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle.
- * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle.
- * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle.
- * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle.
- * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_manual_control_send(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[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN];
-	_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;
-	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
-
-// MESSAGE MANUAL_CONTROL UNPACKING
-
-
-/**
- * @brief Get field target from manual_control message
- *
- * @return The system to be controlled.
- */
-static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field x from manual_control message
- *
- * @return X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle.
- */
-static inline int16_t mavlink_msg_manual_control_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  0);
-}
-
-/**
- * @brief Get field y from manual_control message
- *
- * @return Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle.
- */
-static inline int16_t mavlink_msg_manual_control_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  2);
-}
-
-/**
- * @brief Get field z from manual_control message
- *
- * @return Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle.
- */
-static inline int16_t mavlink_msg_manual_control_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  4);
-}
-
-/**
- * @brief Get field r from manual_control message
- *
- * @return R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle.
- */
-static inline int16_t mavlink_msg_manual_control_get_r(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  6);
-}
-
-/**
- * @brief Get field buttons from manual_control message
- *
- * @return A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.
- */
-static inline uint16_t mavlink_msg_manual_control_get_buttons(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Decode a manual_control message into a struct
- *
- * @param msg The message to decode
- * @param manual_control C-struct to decode the message contents into
- */
-static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* msg, mavlink_manual_control_t* manual_control)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	manual_control->x = mavlink_msg_manual_control_get_x(msg);
-	manual_control->y = mavlink_msg_manual_control_get_y(msg);
-	manual_control->z = mavlink_msg_manual_control_get_z(msg);
-	manual_control->r = mavlink_msg_manual_control_get_r(msg);
-	manual_control->buttons = mavlink_msg_manual_control_get_buttons(msg);
-	manual_control->target = mavlink_msg_manual_control_get_target(msg);
-#else
-	memcpy(manual_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
-#endif
-}
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
deleted file mode 100644
index a694947c1..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE MANUAL_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_MANUAL_SETPOINT 81
-
-typedef struct __mavlink_manual_setpoint_t
-{
- uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot
- float roll; ///< Desired roll rate in radians per second
- float pitch; ///< Desired pitch rate in radians per second
- float yaw; ///< Desired yaw rate in radians per second
- float thrust; ///< Collective thrust, normalized to 0 .. 1
- uint8_t mode_switch; ///< Flight mode switch position, 0.. 255
- uint8_t manual_override_switch; ///< Override mode switch position, 0.. 255
-} mavlink_manual_setpoint_t;
-
-#define MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN 22
-#define MAVLINK_MSG_ID_81_LEN 22
-
-#define MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC 106
-#define MAVLINK_MSG_ID_81_CRC 106
-
-
-
-#define MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT { \
-	"MANUAL_SETPOINT", \
-	7, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_manual_setpoint_t, time_boot_ms) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_manual_setpoint_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_manual_setpoint_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_manual_setpoint_t, yaw) }, \
-         { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_manual_setpoint_t, thrust) }, \
-         { "mode_switch", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_manual_setpoint_t, mode_switch) }, \
-         { "manual_override_switch", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_manual_setpoint_t, manual_override_switch) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a manual_setpoint 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 in milliseconds since system boot
- * @param roll Desired roll rate in radians per second
- * @param pitch Desired pitch rate in radians per second
- * @param yaw Desired yaw rate in radians per second
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @param mode_switch Flight mode switch position, 0.. 255
- * @param manual_override_switch Override mode switch position, 0.. 255
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
-#else
-	mavlink_manual_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a manual_setpoint 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 in milliseconds since system boot
- * @param roll Desired roll rate in radians per second
- * @param pitch Desired pitch rate in radians per second
- * @param yaw Desired yaw rate in radians per second
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @param mode_switch Flight mode switch position, 0.. 255
- * @param manual_override_switch Override mode switch position, 0.. 255
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
-#else
-	mavlink_manual_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a manual_setpoint 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 manual_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_manual_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint)
-{
-	return mavlink_msg_manual_setpoint_pack(system_id, component_id, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch);
-}
-
-/**
- * @brief Encode a manual_setpoint 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 manual_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_manual_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint)
-{
-	return mavlink_msg_manual_setpoint_pack_chan(system_id, component_id, chan, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch);
-}
-
-/**
- * @brief Send a manual_setpoint message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp in milliseconds since system boot
- * @param roll Desired roll rate in radians per second
- * @param pitch Desired pitch rate in radians per second
- * @param yaw Desired yaw rate in radians per second
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @param mode_switch Flight mode switch position, 0.. 255
- * @param manual_override_switch Override mode switch position, 0.. 255
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_manual_setpoint_send(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[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN];
-	_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;
-	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
-
-// MESSAGE MANUAL_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from manual_setpoint message
- *
- * @return Timestamp in milliseconds since system boot
- */
-static inline uint32_t mavlink_msg_manual_setpoint_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field roll from manual_setpoint message
- *
- * @return Desired roll rate in radians per second
- */
-static inline float mavlink_msg_manual_setpoint_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field pitch from manual_setpoint message
- *
- * @return Desired pitch rate in radians per second
- */
-static inline float mavlink_msg_manual_setpoint_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw from manual_setpoint message
- *
- * @return Desired yaw rate in radians per second
- */
-static inline float mavlink_msg_manual_setpoint_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field thrust from manual_setpoint message
- *
- * @return Collective thrust, normalized to 0 .. 1
- */
-static inline float mavlink_msg_manual_setpoint_get_thrust(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field mode_switch from manual_setpoint message
- *
- * @return Flight mode switch position, 0.. 255
- */
-static inline uint8_t mavlink_msg_manual_setpoint_get_mode_switch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  20);
-}
-
-/**
- * @brief Get field manual_override_switch from manual_setpoint message
- *
- * @return Override mode switch position, 0.. 255
- */
-static inline uint8_t mavlink_msg_manual_setpoint_get_manual_override_switch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  21);
-}
-
-/**
- * @brief Decode a manual_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param manual_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_manual_setpoint_decode(const mavlink_message_t* msg, mavlink_manual_setpoint_t* manual_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	manual_setpoint->time_boot_ms = mavlink_msg_manual_setpoint_get_time_boot_ms(msg);
-	manual_setpoint->roll = mavlink_msg_manual_setpoint_get_roll(msg);
-	manual_setpoint->pitch = mavlink_msg_manual_setpoint_get_pitch(msg);
-	manual_setpoint->yaw = mavlink_msg_manual_setpoint_get_yaw(msg);
-	manual_setpoint->thrust = mavlink_msg_manual_setpoint_get_thrust(msg);
-	manual_setpoint->mode_switch = mavlink_msg_manual_setpoint_get_mode_switch(msg);
-	manual_setpoint->manual_override_switch = mavlink_msg_manual_setpoint_get_manual_override_switch(msg);
-#else
-	memcpy(manual_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
-#endif
-}
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
deleted file mode 100644
index 5f79329c2..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h
+++ /dev/null
@@ -1,237 +0,0 @@
-// MESSAGE MEMORY_VECT PACKING
-
-#define MAVLINK_MSG_ID_MEMORY_VECT 249
-
-typedef struct __mavlink_memory_vect_t
-{
- uint16_t address; ///< Starting address of the debug variables
- uint8_t ver; ///< Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below
- uint8_t type; ///< Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14
- int8_t value[32]; ///< Memory contents at specified address
-} mavlink_memory_vect_t;
-
-#define MAVLINK_MSG_ID_MEMORY_VECT_LEN 36
-#define MAVLINK_MSG_ID_249_LEN 36
-
-#define MAVLINK_MSG_ID_MEMORY_VECT_CRC 204
-#define MAVLINK_MSG_ID_249_CRC 204
-
-#define MAVLINK_MSG_MEMORY_VECT_FIELD_VALUE_LEN 32
-
-#define MAVLINK_MESSAGE_INFO_MEMORY_VECT { \
-	"MEMORY_VECT", \
-	4, \
-	{  { "address", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_memory_vect_t, address) }, \
-         { "ver", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_memory_vect_t, ver) }, \
-         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_memory_vect_t, type) }, \
-         { "value", NULL, MAVLINK_TYPE_INT8_T, 32, 4, offsetof(mavlink_memory_vect_t, value) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a memory_vect 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 address Starting address of the debug variables
- * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below
- * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14
- * @param value Memory contents at specified address
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t address, uint8_t ver, uint8_t type, const int8_t *value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
-#else
-	mavlink_memory_vect_t packet;
-	packet.address = address;
-	packet.ver = ver;
-	packet.type = type;
-	mav_array_memcpy(packet.value, value, sizeof(int8_t)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a memory_vect 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 address Starting address of the debug variables
- * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below
- * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14
- * @param value Memory contents at specified address
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t address,uint8_t ver,uint8_t type,const int8_t *value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
-#else
-	mavlink_memory_vect_t packet;
-	packet.address = address;
-	packet.ver = ver;
-	packet.type = type;
-	mav_array_memcpy(packet.value, value, sizeof(int8_t)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a memory_vect 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 memory_vect C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect)
-{
-	return mavlink_msg_memory_vect_pack(system_id, component_id, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value);
-}
-
-/**
- * @brief Encode a memory_vect 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 memory_vect C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_memory_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect)
-{
-	return mavlink_msg_memory_vect_pack_chan(system_id, component_id, chan, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value);
-}
-
-/**
- * @brief Send a memory_vect message
- * @param chan MAVLink channel to send the message
- *
- * @param address Starting address of the debug variables
- * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below
- * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14
- * @param value Memory contents at specified address
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_memory_vect_send(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[MAVLINK_MSG_ID_MEMORY_VECT_LEN];
-	_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;
-	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
-
-// MESSAGE MEMORY_VECT UNPACKING
-
-
-/**
- * @brief Get field address from memory_vect message
- *
- * @return Starting address of the debug variables
- */
-static inline uint16_t mavlink_msg_memory_vect_get_address(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field ver from memory_vect message
- *
- * @return Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below
- */
-static inline uint8_t mavlink_msg_memory_vect_get_ver(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field type from memory_vect message
- *
- * @return Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14
- */
-static inline uint8_t mavlink_msg_memory_vect_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field value from memory_vect message
- *
- * @return Memory contents at specified address
- */
-static inline uint16_t mavlink_msg_memory_vect_get_value(const mavlink_message_t* msg, int8_t *value)
-{
-	return _MAV_RETURN_int8_t_array(msg, value, 32,  4);
-}
-
-/**
- * @brief Decode a memory_vect message into a struct
- *
- * @param msg The message to decode
- * @param memory_vect C-struct to decode the message contents into
- */
-static inline void mavlink_msg_memory_vect_decode(const mavlink_message_t* msg, mavlink_memory_vect_t* memory_vect)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	memory_vect->address = mavlink_msg_memory_vect_get_address(msg);
-	memory_vect->ver = mavlink_msg_memory_vect_get_ver(msg);
-	memory_vect->type = mavlink_msg_memory_vect_get_type(msg);
-	mavlink_msg_memory_vect_get_value(msg, memory_vect->value);
-#else
-	memcpy(memory_vect, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MEMORY_VECT_LEN);
-#endif
-}
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
deleted file mode 100644
index 7421d8394..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE MISSION_ACK PACKING
-
-#define MAVLINK_MSG_ID_MISSION_ACK 47
-
-typedef struct __mavlink_mission_ack_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t type; ///< See MAV_MISSION_RESULT enum
-} mavlink_mission_ack_t;
-
-#define MAVLINK_MSG_ID_MISSION_ACK_LEN 3
-#define MAVLINK_MSG_ID_47_LEN 3
-
-#define MAVLINK_MSG_ID_MISSION_ACK_CRC 153
-#define MAVLINK_MSG_ID_47_CRC 153
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_ACK { \
-	"MISSION_ACK", \
-	3, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \
-         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_ack 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 target_system System ID
- * @param target_component Component ID
- * @param type See MAV_MISSION_RESULT enum
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t type)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, type);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN);
-#else
-	mavlink_mission_ack_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.type = type;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_ack 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 target_system System ID
- * @param target_component Component ID
- * @param type See MAV_MISSION_RESULT enum
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t type)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, type);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN);
-#else
-	mavlink_mission_ack_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.type = type;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_ack 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 mission_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack)
-{
-	return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type);
-}
-
-/**
- * @brief Encode a mission_ack 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 mission_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack)
-{
-	return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type);
-}
-
-/**
- * @brief Send a mission_ack message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param type See MAV_MISSION_RESULT enum
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_ack_send(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[MAVLINK_MSG_ID_MISSION_ACK_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_ACK UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_ack message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_ack_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from mission_ack message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_ack_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field type from mission_ack message
- *
- * @return See MAV_MISSION_RESULT enum
- */
-static inline uint8_t mavlink_msg_mission_ack_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a mission_ack message into a struct
- *
- * @param msg The message to decode
- * @param mission_ack C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_ack_decode(const mavlink_message_t* msg, mavlink_mission_ack_t* mission_ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_ack->target_system = mavlink_msg_mission_ack_get_target_system(msg);
-	mission_ack->target_component = mavlink_msg_mission_ack_get_target_component(msg);
-	mission_ack->type = mavlink_msg_mission_ack_get_type(msg);
-#else
-	memcpy(mission_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ACK_LEN);
-#endif
-}
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
deleted file mode 100644
index 8f441c8e5..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE MISSION_CLEAR_ALL PACKING
-
-#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL 45
-
-typedef struct __mavlink_mission_clear_all_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mission_clear_all_t;
-
-#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN 2
-#define MAVLINK_MSG_ID_45_LEN 2
-
-#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC 232
-#define MAVLINK_MSG_ID_45_CRC 232
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL { \
-	"MISSION_CLEAR_ALL", \
-	2, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_clear_all_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_clear_all_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_clear_all 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
-#else
-	mavlink_mission_clear_all_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_clear_all 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
-#else
-	mavlink_mission_clear_all_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_clear_all 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 mission_clear_all C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all)
-{
-	return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component);
-}
-
-/**
- * @brief Encode a mission_clear_all 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 mission_clear_all C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all)
-{
-	return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component);
-}
-
-/**
- * @brief Send a mission_clear_all message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_CLEAR_ALL UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_clear_all message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_clear_all_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from mission_clear_all message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_clear_all_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Decode a mission_clear_all message into a struct
- *
- * @param msg The message to decode
- * @param mission_clear_all C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_clear_all_decode(const mavlink_message_t* msg, mavlink_mission_clear_all_t* mission_clear_all)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_clear_all->target_system = mavlink_msg_mission_clear_all_get_target_system(msg);
-	mission_clear_all->target_component = mavlink_msg_mission_clear_all_get_target_component(msg);
-#else
-	memcpy(mission_clear_all, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
-#endif
-}
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
deleted file mode 100644
index eac779306..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE MISSION_COUNT PACKING
-
-#define MAVLINK_MSG_ID_MISSION_COUNT 44
-
-typedef struct __mavlink_mission_count_t
-{
- uint16_t count; ///< Number of mission items in the sequence
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mission_count_t;
-
-#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 4
-#define MAVLINK_MSG_ID_44_LEN 4
-
-#define MAVLINK_MSG_ID_MISSION_COUNT_CRC 221
-#define MAVLINK_MSG_ID_44_CRC 221
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \
-	"MISSION_COUNT", \
-	3, \
-	{  { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_count 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 target_system System ID
- * @param target_component Component ID
- * @param count Number of mission items in the sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint16_t count)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN];
-	_mav_put_uint16_t(buf, 0, count);
-	_mav_put_uint8_t(buf, 2, target_system);
-	_mav_put_uint8_t(buf, 3, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
-#else
-	mavlink_mission_count_t packet;
-	packet.count = count;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_count 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 target_system System ID
- * @param target_component Component ID
- * @param count Number of mission items in the sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint16_t count)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN];
-	_mav_put_uint16_t(buf, 0, count);
-	_mav_put_uint8_t(buf, 2, target_system);
-	_mav_put_uint8_t(buf, 3, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
-#else
-	mavlink_mission_count_t packet;
-	packet.count = count;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_count 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 mission_count C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count)
-{
-	return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count);
-}
-
-/**
- * @brief Encode a mission_count 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 mission_count C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count)
-{
-	return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count);
-}
-
-/**
- * @brief Send a mission_count message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param count Number of mission items in the sequence
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_count_send(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[MAVLINK_MSG_ID_MISSION_COUNT_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_COUNT UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_count message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_count_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field target_component from mission_count message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_count_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field count from mission_count message
- *
- * @return Number of mission items in the sequence
- */
-static inline uint16_t mavlink_msg_mission_count_get_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Decode a mission_count message into a struct
- *
- * @param msg The message to decode
- * @param mission_count C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_count_decode(const mavlink_message_t* msg, mavlink_mission_count_t* mission_count)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_count->count = mavlink_msg_mission_count_get_count(msg);
-	mission_count->target_system = mavlink_msg_mission_count_get_target_system(msg);
-	mission_count->target_component = mavlink_msg_mission_count_get_target_component(msg);
-#else
-	memcpy(mission_count, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_COUNT_LEN);
-#endif
-}
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
deleted file mode 100644
index dbcdbd3f9..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h
+++ /dev/null
@@ -1,177 +0,0 @@
-// MESSAGE MISSION_CURRENT PACKING
-
-#define MAVLINK_MSG_ID_MISSION_CURRENT 42
-
-typedef struct __mavlink_mission_current_t
-{
- uint16_t seq; ///< Sequence
-} mavlink_mission_current_t;
-
-#define MAVLINK_MSG_ID_MISSION_CURRENT_LEN 2
-#define MAVLINK_MSG_ID_42_LEN 2
-
-#define MAVLINK_MSG_ID_MISSION_CURRENT_CRC 28
-#define MAVLINK_MSG_ID_42_CRC 28
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \
-	"MISSION_CURRENT", \
-	1, \
-	{  { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_current 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 seq Sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN];
-	_mav_put_uint16_t(buf, 0, seq);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
-#else
-	mavlink_mission_current_t packet;
-	packet.seq = seq;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_current 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 seq Sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN];
-	_mav_put_uint16_t(buf, 0, seq);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
-#else
-	mavlink_mission_current_t packet;
-	packet.seq = seq;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_current 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 mission_current C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current)
-{
-	return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq);
-}
-
-/**
- * @brief Encode a mission_current 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 mission_current C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current)
-{
-	return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq);
-}
-
-/**
- * @brief Send a mission_current message
- * @param chan MAVLink channel to send the message
- *
- * @param seq Sequence
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_CURRENT UNPACKING
-
-
-/**
- * @brief Get field seq from mission_current message
- *
- * @return Sequence
- */
-static inline uint16_t mavlink_msg_mission_current_get_seq(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Decode a mission_current message into a struct
- *
- * @param msg The message to decode
- * @param mission_current C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_current_decode(const mavlink_message_t* msg, mavlink_mission_current_t* mission_current)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_current->seq = mavlink_msg_mission_current_get_seq(msg);
-#else
-	memcpy(mission_current, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
-#endif
-}
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
deleted file mode 100644
index a8d60eca7..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h
+++ /dev/null
@@ -1,463 +0,0 @@
-// MESSAGE MISSION_ITEM PACKING
-
-#define MAVLINK_MSG_ID_MISSION_ITEM 39
-
-typedef struct __mavlink_mission_item_t
-{
- float param1; ///< PARAM1, see MAV_CMD enum
- float param2; ///< PARAM2, see MAV_CMD enum
- float param3; ///< PARAM3, see MAV_CMD enum
- float param4; ///< PARAM4, see MAV_CMD enum
- float x; ///< PARAM5 / local: x position, global: latitude
- float y; ///< PARAM6 / y position: global: longitude
- float z; ///< PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
- uint16_t seq; ///< Sequence
- uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
- uint8_t current; ///< false:0, true:1
- uint8_t autocontinue; ///< autocontinue to next wp
-} mavlink_mission_item_t;
-
-#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 37
-#define MAVLINK_MSG_ID_39_LEN 37
-
-#define MAVLINK_MSG_ID_MISSION_ITEM_CRC 254
-#define MAVLINK_MSG_ID_39_CRC 254
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_ITEM { \
-	"MISSION_ITEM", \
-	14, \
-	{  { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mission_item_t, param1) }, \
-         { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mission_item_t, param2) }, \
-         { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mission_item_t, param3) }, \
-         { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mission_item_t, param4) }, \
-         { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mission_item_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mission_item_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_t, z) }, \
-         { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_t, seq) }, \
-         { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_mission_item_t, command) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, target_component) }, \
-         { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_mission_item_t, frame) }, \
-         { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_mission_item_t, current) }, \
-         { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_mission_item_t, autocontinue) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_item 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 target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
- * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
- * @param current false:0, true:1
- * @param autocontinue autocontinue to next wp
- * @param param1 PARAM1, see MAV_CMD enum
- * @param param2 PARAM2, see MAV_CMD enum
- * @param param3 PARAM3, see MAV_CMD enum
- * @param param4 PARAM4, see MAV_CMD enum
- * @param x PARAM5 / local: x position, global: latitude
- * @param y PARAM6 / y position: global: longitude
- * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MISSION_ITEM_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
-#else
-	mavlink_mission_item_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_item 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 target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
- * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
- * @param current false:0, true:1
- * @param autocontinue autocontinue to next wp
- * @param param1 PARAM1, see MAV_CMD enum
- * @param param2 PARAM2, see MAV_CMD enum
- * @param param3 PARAM3, see MAV_CMD enum
- * @param param4 PARAM4, see MAV_CMD enum
- * @param x PARAM5 / local: x position, global: latitude
- * @param y PARAM6 / y position: global: longitude
- * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MISSION_ITEM_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
-#else
-	mavlink_mission_item_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_item 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 mission_item C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item)
-{
-	return mavlink_msg_mission_item_pack(system_id, component_id, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z);
-}
-
-/**
- * @brief Encode a mission_item 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 mission_item C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item)
-{
-	return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z);
-}
-
-/**
- * @brief Send a mission_item message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- * @param frame The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
- * @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
- * @param current false:0, true:1
- * @param autocontinue autocontinue to next wp
- * @param param1 PARAM1, see MAV_CMD enum
- * @param param2 PARAM2, see MAV_CMD enum
- * @param param3 PARAM3, see MAV_CMD enum
- * @param param4 PARAM4, see MAV_CMD enum
- * @param x PARAM5 / local: x position, global: latitude
- * @param y PARAM6 / y position: global: longitude
- * @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_item_send(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[MAVLINK_MSG_ID_MISSION_ITEM_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_ITEM UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_item message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_item_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  32);
-}
-
-/**
- * @brief Get field target_component from mission_item message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_item_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  33);
-}
-
-/**
- * @brief Get field seq from mission_item message
- *
- * @return Sequence
- */
-static inline uint16_t mavlink_msg_mission_item_get_seq(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  28);
-}
-
-/**
- * @brief Get field frame from mission_item message
- *
- * @return The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
- */
-static inline uint8_t mavlink_msg_mission_item_get_frame(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  34);
-}
-
-/**
- * @brief Get field command from mission_item message
- *
- * @return The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
- */
-static inline uint16_t mavlink_msg_mission_item_get_command(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  30);
-}
-
-/**
- * @brief Get field current from mission_item message
- *
- * @return false:0, true:1
- */
-static inline uint8_t mavlink_msg_mission_item_get_current(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  35);
-}
-
-/**
- * @brief Get field autocontinue from mission_item message
- *
- * @return autocontinue to next wp
- */
-static inline uint8_t mavlink_msg_mission_item_get_autocontinue(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  36);
-}
-
-/**
- * @brief Get field param1 from mission_item message
- *
- * @return PARAM1, see MAV_CMD enum
- */
-static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field param2 from mission_item message
- *
- * @return PARAM2, see MAV_CMD enum
- */
-static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field param3 from mission_item message
- *
- * @return PARAM3, see MAV_CMD enum
- */
-static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field param4 from mission_item message
- *
- * @return PARAM4, see MAV_CMD enum
- */
-static inline float mavlink_msg_mission_item_get_param4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field x from mission_item message
- *
- * @return PARAM5 / local: x position, global: latitude
- */
-static inline float mavlink_msg_mission_item_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field y from mission_item message
- *
- * @return PARAM6 / y position: global: longitude
- */
-static inline float mavlink_msg_mission_item_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field z from mission_item message
- *
- * @return PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
- */
-static inline float mavlink_msg_mission_item_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a mission_item message into a struct
- *
- * @param msg The message to decode
- * @param mission_item C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_item_decode(const mavlink_message_t* msg, mavlink_mission_item_t* mission_item)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_item->param1 = mavlink_msg_mission_item_get_param1(msg);
-	mission_item->param2 = mavlink_msg_mission_item_get_param2(msg);
-	mission_item->param3 = mavlink_msg_mission_item_get_param3(msg);
-	mission_item->param4 = mavlink_msg_mission_item_get_param4(msg);
-	mission_item->x = mavlink_msg_mission_item_get_x(msg);
-	mission_item->y = mavlink_msg_mission_item_get_y(msg);
-	mission_item->z = mavlink_msg_mission_item_get_z(msg);
-	mission_item->seq = mavlink_msg_mission_item_get_seq(msg);
-	mission_item->command = mavlink_msg_mission_item_get_command(msg);
-	mission_item->target_system = mavlink_msg_mission_item_get_target_system(msg);
-	mission_item->target_component = mavlink_msg_mission_item_get_target_component(msg);
-	mission_item->frame = mavlink_msg_mission_item_get_frame(msg);
-	mission_item->current = mavlink_msg_mission_item_get_current(msg);
-	mission_item->autocontinue = mavlink_msg_mission_item_get_autocontinue(msg);
-#else
-	memcpy(mission_item, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ITEM_LEN);
-#endif
-}
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
deleted file mode 100644
index f3744fde6..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h
+++ /dev/null
@@ -1,177 +0,0 @@
-// MESSAGE MISSION_ITEM_REACHED PACKING
-
-#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED 46
-
-typedef struct __mavlink_mission_item_reached_t
-{
- uint16_t seq; ///< Sequence
-} mavlink_mission_item_reached_t;
-
-#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN 2
-#define MAVLINK_MSG_ID_46_LEN 2
-
-#define MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC 11
-#define MAVLINK_MSG_ID_46_CRC 11
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED { \
-	"MISSION_ITEM_REACHED", \
-	1, \
-	{  { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_item_reached_t, seq) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_item_reached 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 seq Sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN];
-	_mav_put_uint16_t(buf, 0, seq);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
-#else
-	mavlink_mission_item_reached_t packet;
-	packet.seq = seq;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_item_reached 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 seq Sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN];
-	_mav_put_uint16_t(buf, 0, seq);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
-#else
-	mavlink_mission_item_reached_t packet;
-	packet.seq = seq;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_item_reached 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 mission_item_reached C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached)
-{
-	return mavlink_msg_mission_item_reached_pack(system_id, component_id, msg, mission_item_reached->seq);
-}
-
-/**
- * @brief Encode a mission_item_reached 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 mission_item_reached C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_item_reached_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached)
-{
-	return mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, chan, msg, mission_item_reached->seq);
-}
-
-/**
- * @brief Send a mission_item_reached message
- * @param chan MAVLink channel to send the message
- *
- * @param seq Sequence
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_ITEM_REACHED UNPACKING
-
-
-/**
- * @brief Get field seq from mission_item_reached message
- *
- * @return Sequence
- */
-static inline uint16_t mavlink_msg_mission_item_reached_get_seq(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Decode a mission_item_reached message into a struct
- *
- * @param msg The message to decode
- * @param mission_item_reached C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_item_reached_decode(const mavlink_message_t* msg, mavlink_mission_item_reached_t* mission_item_reached)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_item_reached->seq = mavlink_msg_mission_item_reached_get_seq(msg);
-#else
-	memcpy(mission_item_reached, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
-#endif
-}
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
deleted file mode 100644
index ac84e3554..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE MISSION_REQUEST PACKING
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST 40
-
-typedef struct __mavlink_mission_request_t
-{
- uint16_t seq; ///< Sequence
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mission_request_t;
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST_LEN 4
-#define MAVLINK_MSG_ID_40_LEN 4
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST_CRC 230
-#define MAVLINK_MSG_ID_40_CRC 230
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST { \
-	"MISSION_REQUEST", \
-	3, \
-	{  { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_t, seq) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_request 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 target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN];
-	_mav_put_uint16_t(buf, 0, seq);
-	_mav_put_uint8_t(buf, 2, target_system);
-	_mav_put_uint8_t(buf, 3, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
-#else
-	mavlink_mission_request_t packet;
-	packet.seq = seq;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_request 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 target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN];
-	_mav_put_uint16_t(buf, 0, seq);
-	_mav_put_uint8_t(buf, 2, target_system);
-	_mav_put_uint8_t(buf, 3, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
-#else
-	mavlink_mission_request_t packet;
-	packet.seq = seq;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_request 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 mission_request C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request)
-{
-	return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq);
-}
-
-/**
- * @brief Encode a mission_request 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 mission_request C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request)
-{
-	return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq);
-}
-
-/**
- * @brief Send a mission_request message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_request_send(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[MAVLINK_MSG_ID_MISSION_REQUEST_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_REQUEST UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_request message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_request_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field target_component from mission_request message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_request_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field seq from mission_request message
- *
- * @return Sequence
- */
-static inline uint16_t mavlink_msg_mission_request_get_seq(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Decode a mission_request message into a struct
- *
- * @param msg The message to decode
- * @param mission_request C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_request_decode(const mavlink_message_t* msg, mavlink_mission_request_t* mission_request)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_request->seq = mavlink_msg_mission_request_get_seq(msg);
-	mission_request->target_system = mavlink_msg_mission_request_get_target_system(msg);
-	mission_request->target_component = mavlink_msg_mission_request_get_target_component(msg);
-#else
-	memcpy(mission_request, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
-#endif
-}
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
deleted file mode 100644
index d999babdb..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE MISSION_REQUEST_LIST PACKING
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST 43
-
-typedef struct __mavlink_mission_request_list_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mission_request_list_t;
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN 2
-#define MAVLINK_MSG_ID_43_LEN 2
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC 132
-#define MAVLINK_MSG_ID_43_CRC 132
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST { \
-	"MISSION_REQUEST_LIST", \
-	2, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_request_list_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_request_list_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_request_list 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
-#else
-	mavlink_mission_request_list_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_request_list 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
-#else
-	mavlink_mission_request_list_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_request_list 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 mission_request_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list)
-{
-	return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component);
-}
-
-/**
- * @brief Encode a mission_request_list 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 mission_request_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list)
-{
-	return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component);
-}
-
-/**
- * @brief Send a mission_request_list message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_REQUEST_LIST UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_request_list message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_request_list_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from mission_request_list message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_request_list_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Decode a mission_request_list message into a struct
- *
- * @param msg The message to decode
- * @param mission_request_list C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_request_list_decode(const mavlink_message_t* msg, mavlink_mission_request_list_t* mission_request_list)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_request_list->target_system = mavlink_msg_mission_request_list_get_target_system(msg);
-	mission_request_list->target_component = mavlink_msg_mission_request_list_get_target_component(msg);
-#else
-	memcpy(mission_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
-#endif
-}
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
deleted file mode 100644
index 35c7e1285..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE MISSION_REQUEST_PARTIAL_LIST PACKING
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST 37
-
-typedef struct __mavlink_mission_request_partial_list_t
-{
- int16_t start_index; ///< Start index, 0 by default
- int16_t end_index; ///< End index, -1 by default (-1: send list to end). Else a valid index of the list
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mission_request_partial_list_t;
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN 6
-#define MAVLINK_MSG_ID_37_LEN 6
-
-#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC 212
-#define MAVLINK_MSG_ID_37_CRC 212
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST { \
-	"MISSION_REQUEST_PARTIAL_LIST", \
-	4, \
-	{  { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_request_partial_list_t, start_index) }, \
-         { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_request_partial_list_t, end_index) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_partial_list_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_request_partial_list_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_request_partial_list 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 target_system System ID
- * @param target_component Component ID
- * @param start_index Start index, 0 by default
- * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
-#else
-	mavlink_mission_request_partial_list_t packet;
-	packet.start_index = start_index;
-	packet.end_index = end_index;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_request_partial_list 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 target_system System ID
- * @param target_component Component ID
- * @param start_index Start index, 0 by default
- * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
-#else
-	mavlink_mission_request_partial_list_t packet;
-	packet.start_index = start_index;
-	packet.end_index = end_index;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_request_partial_list 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 mission_request_partial_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list)
-{
-	return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index);
-}
-
-/**
- * @brief Encode a mission_request_partial_list 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 mission_request_partial_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list)
-{
-	return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index);
-}
-
-/**
- * @brief Send a mission_request_partial_list message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param start_index Start index, 0 by default
- * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_request_partial_list_send(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[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_REQUEST_PARTIAL_LIST UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_request_partial_list message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_request_partial_list_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field target_component from mission_request_partial_list message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_request_partial_list_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field start_index from mission_request_partial_list message
- *
- * @return Start index, 0 by default
- */
-static inline int16_t mavlink_msg_mission_request_partial_list_get_start_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  0);
-}
-
-/**
- * @brief Get field end_index from mission_request_partial_list message
- *
- * @return End index, -1 by default (-1: send list to end). Else a valid index of the list
- */
-static inline int16_t mavlink_msg_mission_request_partial_list_get_end_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  2);
-}
-
-/**
- * @brief Decode a mission_request_partial_list message into a struct
- *
- * @param msg The message to decode
- * @param mission_request_partial_list C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_request_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_request_partial_list_t* mission_request_partial_list)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_request_partial_list->start_index = mavlink_msg_mission_request_partial_list_get_start_index(msg);
-	mission_request_partial_list->end_index = mavlink_msg_mission_request_partial_list_get_end_index(msg);
-	mission_request_partial_list->target_system = mavlink_msg_mission_request_partial_list_get_target_system(msg);
-	mission_request_partial_list->target_component = mavlink_msg_mission_request_partial_list_get_target_component(msg);
-#else
-	memcpy(mission_request_partial_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
-#endif
-}
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
deleted file mode 100644
index 63b37cf8c..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE MISSION_SET_CURRENT PACKING
-
-#define MAVLINK_MSG_ID_MISSION_SET_CURRENT 41
-
-typedef struct __mavlink_mission_set_current_t
-{
- uint16_t seq; ///< Sequence
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mission_set_current_t;
-
-#define MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN 4
-#define MAVLINK_MSG_ID_41_LEN 4
-
-#define MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC 28
-#define MAVLINK_MSG_ID_41_CRC 28
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT { \
-	"MISSION_SET_CURRENT", \
-	3, \
-	{  { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_set_current_t, seq) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_set_current_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_set_current_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_set_current 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 target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_set_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN];
-	_mav_put_uint16_t(buf, 0, seq);
-	_mav_put_uint8_t(buf, 2, target_system);
-	_mav_put_uint8_t(buf, 3, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
-#else
-	mavlink_mission_set_current_t packet;
-	packet.seq = seq;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_set_current 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 target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_set_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint16_t seq)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN];
-	_mav_put_uint16_t(buf, 0, seq);
-	_mav_put_uint8_t(buf, 2, target_system);
-	_mav_put_uint8_t(buf, 3, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
-#else
-	mavlink_mission_set_current_t packet;
-	packet.seq = seq;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_set_current 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 mission_set_current C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_set_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current)
-{
-	return mavlink_msg_mission_set_current_pack(system_id, component_id, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq);
-}
-
-/**
- * @brief Encode a mission_set_current 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 mission_set_current C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_set_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current)
-{
-	return mavlink_msg_mission_set_current_pack_chan(system_id, component_id, chan, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq);
-}
-
-/**
- * @brief Send a mission_set_current message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param seq Sequence
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_set_current_send(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[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_SET_CURRENT UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_set_current message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_set_current_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field target_component from mission_set_current message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_set_current_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field seq from mission_set_current message
- *
- * @return Sequence
- */
-static inline uint16_t mavlink_msg_mission_set_current_get_seq(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Decode a mission_set_current message into a struct
- *
- * @param msg The message to decode
- * @param mission_set_current C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_set_current_decode(const mavlink_message_t* msg, mavlink_mission_set_current_t* mission_set_current)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_set_current->seq = mavlink_msg_mission_set_current_get_seq(msg);
-	mission_set_current->target_system = mavlink_msg_mission_set_current_get_target_system(msg);
-	mission_set_current->target_component = mavlink_msg_mission_set_current_get_target_component(msg);
-#else
-	memcpy(mission_set_current, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
-#endif
-}
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
deleted file mode 100644
index 7de38bd7b..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE MISSION_WRITE_PARTIAL_LIST PACKING
-
-#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST 38
-
-typedef struct __mavlink_mission_write_partial_list_t
-{
- int16_t start_index; ///< Start index, 0 by default and smaller / equal to the largest index of the current onboard list.
- int16_t end_index; ///< End index, equal or greater than start index.
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mission_write_partial_list_t;
-
-#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN 6
-#define MAVLINK_MSG_ID_38_LEN 6
-
-#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC 9
-#define MAVLINK_MSG_ID_38_CRC 9
-
-
-
-#define MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST { \
-	"MISSION_WRITE_PARTIAL_LIST", \
-	4, \
-	{  { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_write_partial_list_t, start_index) }, \
-         { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_write_partial_list_t, end_index) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_write_partial_list_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_write_partial_list_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mission_write_partial_list 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 target_system System ID
- * @param target_component Component ID
- * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list.
- * @param end_index End index, equal or greater than start index.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
-#else
-	mavlink_mission_write_partial_list_t packet;
-	packet.start_index = start_index;
-	packet.end_index = end_index;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mission_write_partial_list 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 target_system System ID
- * @param target_component Component ID
- * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list.
- * @param end_index End index, equal or greater than start index.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
-#else
-	mavlink_mission_write_partial_list_t packet;
-	packet.start_index = start_index;
-	packet.end_index = end_index;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mission_write_partial_list 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 mission_write_partial_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list)
-{
-	return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index);
-}
-
-/**
- * @brief Encode a mission_write_partial_list 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 mission_write_partial_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list)
-{
-	return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index);
-}
-
-/**
- * @brief Send a mission_write_partial_list message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param start_index Start index, 0 by default and smaller / equal to the largest index of the current onboard list.
- * @param end_index End index, equal or greater than start index.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mission_write_partial_list_send(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[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN];
-	_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;
-	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
-
-// MESSAGE MISSION_WRITE_PARTIAL_LIST UNPACKING
-
-
-/**
- * @brief Get field target_system from mission_write_partial_list message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mission_write_partial_list_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field target_component from mission_write_partial_list message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mission_write_partial_list_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field start_index from mission_write_partial_list message
- *
- * @return Start index, 0 by default and smaller / equal to the largest index of the current onboard list.
- */
-static inline int16_t mavlink_msg_mission_write_partial_list_get_start_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  0);
-}
-
-/**
- * @brief Get field end_index from mission_write_partial_list message
- *
- * @return End index, equal or greater than start index.
- */
-static inline int16_t mavlink_msg_mission_write_partial_list_get_end_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  2);
-}
-
-/**
- * @brief Decode a mission_write_partial_list message into a struct
- *
- * @param msg The message to decode
- * @param mission_write_partial_list C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mission_write_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_write_partial_list_t* mission_write_partial_list)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mission_write_partial_list->start_index = mavlink_msg_mission_write_partial_list_get_start_index(msg);
-	mission_write_partial_list->end_index = mavlink_msg_mission_write_partial_list_get_end_index(msg);
-	mission_write_partial_list->target_system = mavlink_msg_mission_write_partial_list_get_target_system(msg);
-	mission_write_partial_list->target_component = mavlink_msg_mission_write_partial_list_get_target_component(msg);
-#else
-	memcpy(mission_write_partial_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
-#endif
-}
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
deleted file mode 100644
index fbf4f75c9..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h
+++ /dev/null
@@ -1,215 +0,0 @@
-// MESSAGE NAMED_VALUE_FLOAT PACKING
-
-#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 251
-
-typedef struct __mavlink_named_value_float_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float value; ///< Floating point value
- char name[10]; ///< Name of the debug variable
-} mavlink_named_value_float_t;
-
-#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN 18
-#define MAVLINK_MSG_ID_251_LEN 18
-
-#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC 170
-#define MAVLINK_MSG_ID_251_CRC 170
-
-#define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10
-
-#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT { \
-	"NAMED_VALUE_FLOAT", \
-	3, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_named_value_float_t, time_boot_ms) }, \
-         { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_named_value_float_t, value) }, \
-         { "name", NULL, MAVLINK_TYPE_CHAR, 10, 8, offsetof(mavlink_named_value_float_t, name) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a named_value_float 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 name Name of the debug variable
- * @param value Floating point value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint32_t time_boot_ms, const char *name, float value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, value);
-	_mav_put_char_array(buf, 8, name, 10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
-#else
-	mavlink_named_value_float_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.value = value;
-	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a named_value_float 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 name Name of the debug variable
- * @param value Floating point value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint32_t time_boot_ms,const char *name,float value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, value);
-	_mav_put_char_array(buf, 8, name, 10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
-#else
-	mavlink_named_value_float_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.value = value;
-	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a named_value_float 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 named_value_float C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float)
-{
-	return mavlink_msg_named_value_float_pack(system_id, component_id, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value);
-}
-
-/**
- * @brief Encode a named_value_float 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 named_value_float C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_named_value_float_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float)
-{
-	return mavlink_msg_named_value_float_pack_chan(system_id, component_id, chan, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value);
-}
-
-/**
- * @brief Send a named_value_float message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param name Name of the debug variable
- * @param value Floating point value
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN];
-	_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;
-	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
-
-// MESSAGE NAMED_VALUE_FLOAT UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from named_value_float message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_named_value_float_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field name from named_value_float message
- *
- * @return Name of the debug variable
- */
-static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char *name)
-{
-	return _MAV_RETURN_char_array(msg, name, 10,  8);
-}
-
-/**
- * @brief Get field value from named_value_float message
- *
- * @return Floating point value
- */
-static inline float mavlink_msg_named_value_float_get_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Decode a named_value_float message into a struct
- *
- * @param msg The message to decode
- * @param named_value_float C-struct to decode the message contents into
- */
-static inline void mavlink_msg_named_value_float_decode(const mavlink_message_t* msg, mavlink_named_value_float_t* named_value_float)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	named_value_float->time_boot_ms = mavlink_msg_named_value_float_get_time_boot_ms(msg);
-	named_value_float->value = mavlink_msg_named_value_float_get_value(msg);
-	mavlink_msg_named_value_float_get_name(msg, named_value_float->name);
-#else
-	memcpy(named_value_float, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
-#endif
-}
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
deleted file mode 100644
index 052f24793..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h
+++ /dev/null
@@ -1,215 +0,0 @@
-// MESSAGE NAMED_VALUE_INT PACKING
-
-#define MAVLINK_MSG_ID_NAMED_VALUE_INT 252
-
-typedef struct __mavlink_named_value_int_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- int32_t value; ///< Signed integer value
- char name[10]; ///< Name of the debug variable
-} mavlink_named_value_int_t;
-
-#define MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN 18
-#define MAVLINK_MSG_ID_252_LEN 18
-
-#define MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC 44
-#define MAVLINK_MSG_ID_252_CRC 44
-
-#define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10
-
-#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT { \
-	"NAMED_VALUE_INT", \
-	3, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_named_value_int_t, time_boot_ms) }, \
-         { "value", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_named_value_int_t, value) }, \
-         { "name", NULL, MAVLINK_TYPE_CHAR, 10, 8, offsetof(mavlink_named_value_int_t, name) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a named_value_int 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 name Name of the debug variable
- * @param value Signed integer value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint32_t time_boot_ms, const char *name, int32_t value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_int32_t(buf, 4, value);
-	_mav_put_char_array(buf, 8, name, 10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
-#else
-	mavlink_named_value_int_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.value = value;
-	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a named_value_int 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 name Name of the debug variable
- * @param value Signed integer value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint32_t time_boot_ms,const char *name,int32_t value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_int32_t(buf, 4, value);
-	_mav_put_char_array(buf, 8, name, 10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
-#else
-	mavlink_named_value_int_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.value = value;
-	mav_array_memcpy(packet.name, name, sizeof(char)*10);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a named_value_int 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 named_value_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int)
-{
-	return mavlink_msg_named_value_int_pack(system_id, component_id, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value);
-}
-
-/**
- * @brief Encode a named_value_int 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 named_value_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_named_value_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int)
-{
-	return mavlink_msg_named_value_int_pack_chan(system_id, component_id, chan, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value);
-}
-
-/**
- * @brief Send a named_value_int message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param name Name of the debug variable
- * @param value Signed integer value
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_named_value_int_send(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[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN];
-	_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;
-	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
-
-// MESSAGE NAMED_VALUE_INT UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from named_value_int message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_named_value_int_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field name from named_value_int message
- *
- * @return Name of the debug variable
- */
-static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char *name)
-{
-	return _MAV_RETURN_char_array(msg, name, 10,  8);
-}
-
-/**
- * @brief Get field value from named_value_int message
- *
- * @return Signed integer value
- */
-static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Decode a named_value_int message into a struct
- *
- * @param msg The message to decode
- * @param named_value_int C-struct to decode the message contents into
- */
-static inline void mavlink_msg_named_value_int_decode(const mavlink_message_t* msg, mavlink_named_value_int_t* named_value_int)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	named_value_int->time_boot_ms = mavlink_msg_named_value_int_get_time_boot_ms(msg);
-	named_value_int->value = mavlink_msg_named_value_int_get_value(msg);
-	mavlink_msg_named_value_int_get_name(msg, named_value_int->name);
-#else
-	memcpy(named_value_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
-#endif
-}
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
deleted file mode 100644
index e95c144de..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h
+++ /dev/null
@@ -1,331 +0,0 @@
-// MESSAGE NAV_CONTROLLER_OUTPUT PACKING
-
-#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62
-
-typedef struct __mavlink_nav_controller_output_t
-{
- float nav_roll; ///< Current desired roll in degrees
- float nav_pitch; ///< Current desired pitch in degrees
- float alt_error; ///< Current altitude error in meters
- float aspd_error; ///< Current airspeed error in meters/second
- float xtrack_error; ///< Current crosstrack error on x-y plane in meters
- int16_t nav_bearing; ///< Current desired heading in degrees
- int16_t target_bearing; ///< Bearing to current MISSION/target in degrees
- uint16_t wp_dist; ///< Distance to active MISSION in meters
-} mavlink_nav_controller_output_t;
-
-#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26
-#define MAVLINK_MSG_ID_62_LEN 26
-
-#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC 183
-#define MAVLINK_MSG_ID_62_CRC 183
-
-
-
-#define MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT { \
-	"NAV_CONTROLLER_OUTPUT", \
-	8, \
-	{  { "nav_roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_nav_controller_output_t, nav_roll) }, \
-         { "nav_pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_nav_controller_output_t, nav_pitch) }, \
-         { "alt_error", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_nav_controller_output_t, alt_error) }, \
-         { "aspd_error", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_nav_controller_output_t, aspd_error) }, \
-         { "xtrack_error", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_nav_controller_output_t, xtrack_error) }, \
-         { "nav_bearing", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_nav_controller_output_t, nav_bearing) }, \
-         { "target_bearing", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_nav_controller_output_t, target_bearing) }, \
-         { "wp_dist", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_nav_controller_output_t, wp_dist) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a nav_controller_output 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 nav_roll Current desired roll in degrees
- * @param nav_pitch Current desired pitch in degrees
- * @param nav_bearing Current desired heading in degrees
- * @param target_bearing Bearing to current MISSION/target in degrees
- * @param wp_dist Distance to active MISSION in meters
- * @param alt_error Current altitude error in meters
- * @param aspd_error Current airspeed error in meters/second
- * @param xtrack_error Current crosstrack error on x-y plane in meters
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
-#else
-	mavlink_nav_controller_output_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a nav_controller_output 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 nav_roll Current desired roll in degrees
- * @param nav_pitch Current desired pitch in degrees
- * @param nav_bearing Current desired heading in degrees
- * @param target_bearing Bearing to current MISSION/target in degrees
- * @param wp_dist Distance to active MISSION in meters
- * @param alt_error Current altitude error in meters
- * @param aspd_error Current airspeed error in meters/second
- * @param xtrack_error Current crosstrack error on x-y plane in meters
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
-#else
-	mavlink_nav_controller_output_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a nav_controller_output 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 nav_controller_output C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output)
-{
-	return mavlink_msg_nav_controller_output_pack(system_id, component_id, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error);
-}
-
-/**
- * @brief Encode a nav_controller_output 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 nav_controller_output C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_nav_controller_output_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output)
-{
-	return mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, chan, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error);
-}
-
-/**
- * @brief Send a nav_controller_output message
- * @param chan MAVLink channel to send the message
- *
- * @param nav_roll Current desired roll in degrees
- * @param nav_pitch Current desired pitch in degrees
- * @param nav_bearing Current desired heading in degrees
- * @param target_bearing Bearing to current MISSION/target in degrees
- * @param wp_dist Distance to active MISSION in meters
- * @param alt_error Current altitude error in meters
- * @param aspd_error Current airspeed error in meters/second
- * @param xtrack_error Current crosstrack error on x-y plane in meters
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_nav_controller_output_send(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[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN];
-	_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;
-	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
-
-// MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING
-
-
-/**
- * @brief Get field nav_roll from nav_controller_output message
- *
- * @return Current desired roll in degrees
- */
-static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field nav_pitch from nav_controller_output message
- *
- * @return Current desired pitch in degrees
- */
-static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field nav_bearing from nav_controller_output message
- *
- * @return Current desired heading in degrees
- */
-static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Get field target_bearing from nav_controller_output message
- *
- * @return Bearing to current MISSION/target in degrees
- */
-static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  22);
-}
-
-/**
- * @brief Get field wp_dist from nav_controller_output message
- *
- * @return Distance to active MISSION in meters
- */
-static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  24);
-}
-
-/**
- * @brief Get field alt_error from nav_controller_output message
- *
- * @return Current altitude error in meters
- */
-static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field aspd_error from nav_controller_output message
- *
- * @return Current airspeed error in meters/second
- */
-static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field xtrack_error from nav_controller_output message
- *
- * @return Current crosstrack error on x-y plane in meters
- */
-static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a nav_controller_output message into a struct
- *
- * @param msg The message to decode
- * @param nav_controller_output C-struct to decode the message contents into
- */
-static inline void mavlink_msg_nav_controller_output_decode(const mavlink_message_t* msg, mavlink_nav_controller_output_t* nav_controller_output)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	nav_controller_output->nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg);
-	nav_controller_output->nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg);
-	nav_controller_output->alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg);
-	nav_controller_output->aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg);
-	nav_controller_output->xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(msg);
-	nav_controller_output->nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg);
-	nav_controller_output->target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg);
-	nav_controller_output->wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(msg);
-#else
-	memcpy(nav_controller_output, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
-#endif
-}
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
deleted file mode 100644
index 4debb6e66..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h
+++ /dev/null
@@ -1,282 +0,0 @@
-// MESSAGE OMNIDIRECTIONAL_FLOW PACKING
-
-#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW 106
-
-typedef struct __mavlink_omnidirectional_flow_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
- float front_distance_m; ///< Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance
- int16_t left[10]; ///< Flow in deci pixels (1 = 0.1 pixel) on left hemisphere
- int16_t right[10]; ///< Flow in deci pixels (1 = 0.1 pixel) on right hemisphere
- uint8_t sensor_id; ///< Sensor ID
- uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality
-} mavlink_omnidirectional_flow_t;
-
-#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN 54
-#define MAVLINK_MSG_ID_106_LEN 54
-
-#define MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC 211
-#define MAVLINK_MSG_ID_106_CRC 211
-
-#define MAVLINK_MSG_OMNIDIRECTIONAL_FLOW_FIELD_LEFT_LEN 10
-#define MAVLINK_MSG_OMNIDIRECTIONAL_FLOW_FIELD_RIGHT_LEN 10
-
-#define MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW { \
-	"OMNIDIRECTIONAL_FLOW", \
-	6, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_omnidirectional_flow_t, time_usec) }, \
-         { "front_distance_m", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_omnidirectional_flow_t, front_distance_m) }, \
-         { "left", NULL, MAVLINK_TYPE_INT16_T, 10, 12, offsetof(mavlink_omnidirectional_flow_t, left) }, \
-         { "right", NULL, MAVLINK_TYPE_INT16_T, 10, 32, offsetof(mavlink_omnidirectional_flow_t, right) }, \
-         { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_omnidirectional_flow_t, sensor_id) }, \
-         { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_omnidirectional_flow_t, quality) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a omnidirectional_flow 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_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param sensor_id Sensor ID
- * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere
- * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_omnidirectional_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
-#else
-	mavlink_omnidirectional_flow_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
-#endif
-}
-
-/**
- * @brief Pack a omnidirectional_flow 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_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param sensor_id Sensor ID
- * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere
- * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_omnidirectional_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
-#else
-	mavlink_omnidirectional_flow_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
-#endif
-}
-
-/**
- * @brief Encode a omnidirectional_flow 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 omnidirectional_flow C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_omnidirectional_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow)
-{
-	return mavlink_msg_omnidirectional_flow_pack(system_id, component_id, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m);
-}
-
-/**
- * @brief Encode a omnidirectional_flow 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 omnidirectional_flow C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_omnidirectional_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow)
-{
-	return mavlink_msg_omnidirectional_flow_pack_chan(system_id, component_id, chan, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m);
-}
-
-/**
- * @brief Send a omnidirectional_flow message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param sensor_id Sensor ID
- * @param left Flow in deci pixels (1 = 0.1 pixel) on left hemisphere
- * @param right Flow in deci pixels (1 = 0.1 pixel) on right hemisphere
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param front_distance_m Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_omnidirectional_flow_send(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[MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN];
-	_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;
-	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
-
-// MESSAGE OMNIDIRECTIONAL_FLOW UNPACKING
-
-
-/**
- * @brief Get field time_usec from omnidirectional_flow message
- *
- * @return Timestamp (microseconds, synced to UNIX time or since system boot)
- */
-static inline uint64_t mavlink_msg_omnidirectional_flow_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field sensor_id from omnidirectional_flow message
- *
- * @return Sensor ID
- */
-static inline uint8_t mavlink_msg_omnidirectional_flow_get_sensor_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  52);
-}
-
-/**
- * @brief Get field left from omnidirectional_flow message
- *
- * @return Flow in deci pixels (1 = 0.1 pixel) on left hemisphere
- */
-static inline uint16_t mavlink_msg_omnidirectional_flow_get_left(const mavlink_message_t* msg, int16_t *left)
-{
-	return _MAV_RETURN_int16_t_array(msg, left, 10,  12);
-}
-
-/**
- * @brief Get field right from omnidirectional_flow message
- *
- * @return Flow in deci pixels (1 = 0.1 pixel) on right hemisphere
- */
-static inline uint16_t mavlink_msg_omnidirectional_flow_get_right(const mavlink_message_t* msg, int16_t *right)
-{
-	return _MAV_RETURN_int16_t_array(msg, right, 10,  32);
-}
-
-/**
- * @brief Get field quality from omnidirectional_flow message
- *
- * @return Optical flow quality / confidence. 0: bad, 255: maximum quality
- */
-static inline uint8_t mavlink_msg_omnidirectional_flow_get_quality(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  53);
-}
-
-/**
- * @brief Get field front_distance_m from omnidirectional_flow message
- *
- * @return Front distance in meters. Positive value (including zero): distance known. Negative value: Unknown distance
- */
-static inline float mavlink_msg_omnidirectional_flow_get_front_distance_m(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Decode a omnidirectional_flow message into a struct
- *
- * @param msg The message to decode
- * @param omnidirectional_flow C-struct to decode the message contents into
- */
-static inline void mavlink_msg_omnidirectional_flow_decode(const mavlink_message_t* msg, mavlink_omnidirectional_flow_t* omnidirectional_flow)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	omnidirectional_flow->time_usec = mavlink_msg_omnidirectional_flow_get_time_usec(msg);
-	omnidirectional_flow->front_distance_m = mavlink_msg_omnidirectional_flow_get_front_distance_m(msg);
-	mavlink_msg_omnidirectional_flow_get_left(msg, omnidirectional_flow->left);
-	mavlink_msg_omnidirectional_flow_get_right(msg, omnidirectional_flow->right);
-	omnidirectional_flow->sensor_id = mavlink_msg_omnidirectional_flow_get_sensor_id(msg);
-	omnidirectional_flow->quality = mavlink_msg_omnidirectional_flow_get_quality(msg);
-#else
-	memcpy(omnidirectional_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
-#endif
-}
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
deleted file mode 100644
index cf6db9147..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h
+++ /dev/null
@@ -1,331 +0,0 @@
-// MESSAGE OPTICAL_FLOW PACKING
-
-#define MAVLINK_MSG_ID_OPTICAL_FLOW 100
-
-typedef struct __mavlink_optical_flow_t
-{
- uint64_t time_usec; ///< Timestamp (UNIX)
- float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated
- float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated
- float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels)
- int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels)
- uint8_t sensor_id; ///< Sensor ID
- uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality
-} mavlink_optical_flow_t;
-
-#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 26
-#define MAVLINK_MSG_ID_100_LEN 26
-
-#define MAVLINK_MSG_ID_OPTICAL_FLOW_CRC 175
-#define MAVLINK_MSG_ID_100_CRC 175
-
-
-
-#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \
-	"OPTICAL_FLOW", \
-	8, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \
-         { "flow_comp_m_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_optical_flow_t, flow_comp_m_x) }, \
-         { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_t, flow_comp_m_y) }, \
-         { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_t, ground_distance) }, \
-         { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_optical_flow_t, flow_x) }, \
-         { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_optical_flow_t, flow_y) }, \
-         { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_optical_flow_t, sensor_id) }, \
-         { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_optical_flow_t, quality) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a optical_flow 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_usec Timestamp (UNIX)
- * @param sensor_id Sensor ID
- * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels)
- * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels)
- * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated
- * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
-#else
-	mavlink_optical_flow_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
-#endif
-}
-
-/**
- * @brief Pack a optical_flow 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_usec Timestamp (UNIX)
- * @param sensor_id Sensor ID
- * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels)
- * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels)
- * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated
- * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
-#else
-	mavlink_optical_flow_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
-#endif
-}
-
-/**
- * @brief Encode a optical_flow 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 optical_flow C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow)
-{
-	return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance);
-}
-
-/**
- * @brief Encode a optical_flow 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 optical_flow C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow)
-{
-	return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance);
-}
-
-/**
- * @brief Send a optical_flow message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (UNIX)
- * @param sensor_id Sensor ID
- * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels)
- * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels)
- * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated
- * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated
- * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
- * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_optical_flow_send(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[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN];
-	_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;
-	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
-
-// MESSAGE OPTICAL_FLOW UNPACKING
-
-
-/**
- * @brief Get field time_usec from optical_flow message
- *
- * @return Timestamp (UNIX)
- */
-static inline uint64_t mavlink_msg_optical_flow_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field sensor_id from optical_flow message
- *
- * @return Sensor ID
- */
-static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  24);
-}
-
-/**
- * @brief Get field flow_x from optical_flow message
- *
- * @return Flow in pixels * 10 in x-sensor direction (dezi-pixels)
- */
-static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Get field flow_y from optical_flow message
- *
- * @return Flow in pixels * 10 in y-sensor direction (dezi-pixels)
- */
-static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  22);
-}
-
-/**
- * @brief Get field flow_comp_m_x from optical_flow message
- *
- * @return Flow in meters in x-sensor direction, angular-speed compensated
- */
-static inline float mavlink_msg_optical_flow_get_flow_comp_m_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field flow_comp_m_y from optical_flow message
- *
- * @return Flow in meters in y-sensor direction, angular-speed compensated
- */
-static inline float mavlink_msg_optical_flow_get_flow_comp_m_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field quality from optical_flow message
- *
- * @return Optical flow quality / confidence. 0: bad, 255: maximum quality
- */
-static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  25);
-}
-
-/**
- * @brief Get field ground_distance from optical_flow message
- *
- * @return Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
- */
-static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a optical_flow message into a struct
- *
- * @param msg The message to decode
- * @param optical_flow C-struct to decode the message contents into
- */
-static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, mavlink_optical_flow_t* optical_flow)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	optical_flow->time_usec = mavlink_msg_optical_flow_get_time_usec(msg);
-	optical_flow->flow_comp_m_x = mavlink_msg_optical_flow_get_flow_comp_m_x(msg);
-	optical_flow->flow_comp_m_y = mavlink_msg_optical_flow_get_flow_comp_m_y(msg);
-	optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg);
-	optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg);
-	optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg);
-	optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg);
-	optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg);
-#else
-	memcpy(optical_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
-#endif
-}
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
deleted file mode 100644
index 39e007274..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE PARAM_REQUEST_LIST PACKING
-
-#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21
-
-typedef struct __mavlink_param_request_list_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_param_request_list_t;
-
-#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2
-#define MAVLINK_MSG_ID_21_LEN 2
-
-#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC 159
-#define MAVLINK_MSG_ID_21_CRC 159
-
-
-
-#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST { \
-	"PARAM_REQUEST_LIST", \
-	2, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a param_request_list 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
-#else
-	mavlink_param_request_list_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a param_request_list 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
-#else
-	mavlink_param_request_list_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a param_request_list 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 param_request_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list)
-{
-	return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component);
-}
-
-/**
- * @brief Encode a param_request_list 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 param_request_list C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_param_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list)
-{
-	return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component);
-}
-
-/**
- * @brief Send a param_request_list message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN];
-	_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;
-	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
-
-// MESSAGE PARAM_REQUEST_LIST UNPACKING
-
-
-/**
- * @brief Get field target_system from param_request_list message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from param_request_list message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Decode a param_request_list message into a struct
- *
- * @param msg The message to decode
- * @param param_request_list C-struct to decode the message contents into
- */
-static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t* msg, mavlink_param_request_list_t* param_request_list)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg);
-	param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg);
-#else
-	memcpy(param_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
-#endif
-}
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
deleted file mode 100644
index 5d9113114..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h
+++ /dev/null
@@ -1,237 +0,0 @@
-// MESSAGE PARAM_REQUEST_READ PACKING
-
-#define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20
-
-typedef struct __mavlink_param_request_read_t
-{
- int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
-} mavlink_param_request_read_t;
-
-#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20
-#define MAVLINK_MSG_ID_20_LEN 20
-
-#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC 214
-#define MAVLINK_MSG_ID_20_CRC 214
-
-#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16
-
-#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ { \
-	"PARAM_REQUEST_READ", \
-	4, \
-	{  { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_request_read_t, param_index) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_request_read_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_request_read_t, target_component) }, \
-         { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_request_read_t, param_id) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a param_request_read 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 target_system System ID
- * @param target_component Component ID
- * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
-#else
-	mavlink_param_request_read_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
-#endif
-}
-
-/**
- * @brief Pack a param_request_read 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 target_system System ID
- * @param target_component Component ID
- * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
-#else
-	mavlink_param_request_read_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
-#endif
-}
-
-/**
- * @brief Encode a param_request_read 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 param_request_read C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read)
-{
-	return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index);
-}
-
-/**
- * @brief Encode a param_request_read 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 param_request_read C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_param_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read)
-{
-	return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index);
-}
-
-/**
- * @brief Send a param_request_read message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_param_request_read_send(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[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN];
-	_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;
-	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
-
-// MESSAGE PARAM_REQUEST_READ UNPACKING
-
-
-/**
- * @brief Get field target_system from param_request_read message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field target_component from param_request_read message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field param_id from param_request_read message
- *
- * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- */
-static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char *param_id)
-{
-	return _MAV_RETURN_char_array(msg, param_id, 16,  4);
-}
-
-/**
- * @brief Get field param_index from param_request_read message
- *
- * @return Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)
- */
-static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  0);
-}
-
-/**
- * @brief Decode a param_request_read message into a struct
- *
- * @param msg The message to decode
- * @param param_request_read C-struct to decode the message contents into
- */
-static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t* msg, mavlink_param_request_read_t* param_request_read)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg);
-	param_request_read->target_system = mavlink_msg_param_request_read_get_target_system(msg);
-	param_request_read->target_component = mavlink_msg_param_request_read_get_target_component(msg);
-	mavlink_msg_param_request_read_get_param_id(msg, param_request_read->param_id);
-#else
-	memcpy(param_request_read, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
-#endif
-}
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
deleted file mode 100644
index 1bd1f0059..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h
+++ /dev/null
@@ -1,259 +0,0 @@
-// MESSAGE PARAM_SET PACKING
-
-#define MAVLINK_MSG_ID_PARAM_SET 23
-
-typedef struct __mavlink_param_set_t
-{
- float param_value; ///< Onboard parameter value
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- uint8_t param_type; ///< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
-} mavlink_param_set_t;
-
-#define MAVLINK_MSG_ID_PARAM_SET_LEN 23
-#define MAVLINK_MSG_ID_23_LEN 23
-
-#define MAVLINK_MSG_ID_PARAM_SET_CRC 168
-#define MAVLINK_MSG_ID_23_CRC 168
-
-#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16
-
-#define MAVLINK_MESSAGE_INFO_PARAM_SET { \
-	"PARAM_SET", \
-	5, \
-	{  { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_set_t, param_value) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_set_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_param_set_t, target_component) }, \
-         { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 6, offsetof(mavlink_param_set_t, param_id) }, \
-         { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_param_set_t, param_type) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a param_set 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 target_system System ID
- * @param target_component Component ID
- * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_value Onboard parameter value
- * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_PARAM_SET_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN);
-#else
-	mavlink_param_set_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PARAM_SET;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN);
-#endif
-}
-
-/**
- * @brief Pack a param_set 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 target_system System ID
- * @param target_component Component ID
- * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_value Onboard parameter value
- * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_PARAM_SET_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN);
-#else
-	mavlink_param_set_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PARAM_SET;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN);
-#endif
-}
-
-/**
- * @brief Encode a param_set 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 param_set C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_set_t* param_set)
-{
-	return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type);
-}
-
-/**
- * @brief Encode a param_set 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 param_set C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_param_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_set_t* param_set)
-{
-	return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type);
-}
-
-/**
- * @brief Send a param_set message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_value Onboard parameter value
- * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_param_set_send(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[MAVLINK_MSG_ID_PARAM_SET_LEN];
-	_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;
-	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
-
-// MESSAGE PARAM_SET UNPACKING
-
-
-/**
- * @brief Get field target_system from param_set message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field target_component from param_set message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field param_id from param_set message
- *
- * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- */
-static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char *param_id)
-{
-	return _MAV_RETURN_char_array(msg, param_id, 16,  6);
-}
-
-/**
- * @brief Get field param_value from param_set message
- *
- * @return Onboard parameter value
- */
-static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field param_type from param_set message
- *
- * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
- */
-static inline uint8_t mavlink_msg_param_set_get_param_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  22);
-}
-
-/**
- * @brief Decode a param_set message into a struct
- *
- * @param msg The message to decode
- * @param param_set C-struct to decode the message contents into
- */
-static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, mavlink_param_set_t* param_set)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	param_set->param_value = mavlink_msg_param_set_get_param_value(msg);
-	param_set->target_system = mavlink_msg_param_set_get_target_system(msg);
-	param_set->target_component = mavlink_msg_param_set_get_target_component(msg);
-	mavlink_msg_param_set_get_param_id(msg, param_set->param_id);
-	param_set->param_type = mavlink_msg_param_set_get_param_type(msg);
-#else
-	memcpy(param_set, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_SET_LEN);
-#endif
-}
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
deleted file mode 100644
index 17c759811..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h
+++ /dev/null
@@ -1,259 +0,0 @@
-// MESSAGE PARAM_VALUE PACKING
-
-#define MAVLINK_MSG_ID_PARAM_VALUE 22
-
-typedef struct __mavlink_param_value_t
-{
- float param_value; ///< Onboard parameter value
- uint16_t param_count; ///< Total number of onboard parameters
- uint16_t param_index; ///< Index of this onboard parameter
- char param_id[16]; ///< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- uint8_t param_type; ///< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
-} mavlink_param_value_t;
-
-#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 25
-#define MAVLINK_MSG_ID_22_LEN 25
-
-#define MAVLINK_MSG_ID_PARAM_VALUE_CRC 220
-#define MAVLINK_MSG_ID_22_CRC 220
-
-#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16
-
-#define MAVLINK_MESSAGE_INFO_PARAM_VALUE { \
-	"PARAM_VALUE", \
-	5, \
-	{  { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_value_t, param_value) }, \
-         { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_param_value_t, param_count) }, \
-         { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_param_value_t, param_index) }, \
-         { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 8, offsetof(mavlink_param_value_t, param_id) }, \
-         { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_param_value_t, param_type) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a param_value 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 param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_value Onboard parameter value
- * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
- * @param param_count Total number of onboard parameters
- * @param param_index Index of this onboard parameter
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
-#else
-	mavlink_param_value_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a param_value 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 param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_value Onboard parameter value
- * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
- * @param param_count Total number of onboard parameters
- * @param param_index Index of this onboard parameter
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
-#else
-	mavlink_param_value_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a param_value 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 param_value C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_value_t* param_value)
-{
-	return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index);
-}
-
-/**
- * @brief Encode a param_value 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 param_value C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_param_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_value_t* param_value)
-{
-	return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index);
-}
-
-/**
- * @brief Send a param_value message
- * @param chan MAVLink channel to send the message
- *
- * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- * @param param_value Onboard parameter value
- * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
- * @param param_count Total number of onboard parameters
- * @param param_index Index of this onboard parameter
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_param_value_send(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[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
-	_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;
-	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
-
-// MESSAGE PARAM_VALUE UNPACKING
-
-
-/**
- * @brief Get field param_id from param_value message
- *
- * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
- */
-static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char *param_id)
-{
-	return _MAV_RETURN_char_array(msg, param_id, 16,  8);
-}
-
-/**
- * @brief Get field param_value from param_value message
- *
- * @return Onboard parameter value
- */
-static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field param_type from param_value message
- *
- * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.
- */
-static inline uint8_t mavlink_msg_param_value_get_param_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  24);
-}
-
-/**
- * @brief Get field param_count from param_value message
- *
- * @return Total number of onboard parameters
- */
-static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field param_index from param_value message
- *
- * @return Index of this onboard parameter
- */
-static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Decode a param_value message into a struct
- *
- * @param msg The message to decode
- * @param param_value C-struct to decode the message contents into
- */
-static inline void mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	param_value->param_value = mavlink_msg_param_value_get_param_value(msg);
-	param_value->param_count = mavlink_msg_param_value_get_param_count(msg);
-	param_value->param_index = mavlink_msg_param_value_get_param_index(msg);
-	mavlink_msg_param_value_get_param_id(msg, param_value->param_id);
-	param_value->param_type = mavlink_msg_param_value_get_param_type(msg);
-#else
-	memcpy(param_value, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_VALUE_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h
deleted file mode 100644
index 0c68ca176..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE PING PACKING
-
-#define MAVLINK_MSG_ID_PING 4
-
-typedef struct __mavlink_ping_t
-{
- uint64_t time_usec; ///< Unix timestamp in microseconds
- uint32_t seq; ///< PING sequence
- uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system
- uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system
-} mavlink_ping_t;
-
-#define MAVLINK_MSG_ID_PING_LEN 14
-#define MAVLINK_MSG_ID_4_LEN 14
-
-#define MAVLINK_MSG_ID_PING_CRC 237
-#define MAVLINK_MSG_ID_4_CRC 237
-
-
-
-#define MAVLINK_MESSAGE_INFO_PING { \
-	"PING", \
-	4, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_ping_t, time_usec) }, \
-         { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_ping_t, seq) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_ping_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_ping_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a ping 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_usec Unix timestamp in microseconds
- * @param seq PING sequence
- * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system
- * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_PING_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN);
-#else
-	mavlink_ping_t packet;
-	packet.time_usec = time_usec;
-	packet.seq = seq;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PING;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN);
-#endif
-}
-
-/**
- * @brief Pack a ping 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_usec Unix timestamp in microseconds
- * @param seq PING sequence
- * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system
- * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_PING_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN);
-#else
-	mavlink_ping_t packet;
-	packet.time_usec = time_usec;
-	packet.seq = seq;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PING;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN);
-#endif
-}
-
-/**
- * @brief Encode a ping 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 ping C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ping_t* ping)
-{
-	return mavlink_msg_ping_pack(system_id, component_id, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
-}
-
-/**
- * @brief Encode a ping 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 ping C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ping_t* ping)
-{
-	return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
-}
-
-/**
- * @brief Send a ping message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Unix timestamp in microseconds
- * @param seq PING sequence
- * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system
- * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_ping_send(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[MAVLINK_MSG_ID_PING_LEN];
-	_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;
-	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
-
-// MESSAGE PING UNPACKING
-
-
-/**
- * @brief Get field time_usec from ping message
- *
- * @return Unix timestamp in microseconds
- */
-static inline uint64_t mavlink_msg_ping_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field seq from ping message
- *
- * @return PING sequence
- */
-static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  8);
-}
-
-/**
- * @brief Get field target_system from ping message
- *
- * @return 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system
- */
-static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field target_component from ping message
- *
- * @return 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system
- */
-static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  13);
-}
-
-/**
- * @brief Decode a ping message into a struct
- *
- * @param msg The message to decode
- * @param ping C-struct to decode the message contents into
- */
-static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink_ping_t* ping)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	ping->time_usec = mavlink_msg_ping_get_time_usec(msg);
-	ping->seq = mavlink_msg_ping_get_seq(msg);
-	ping->target_system = mavlink_msg_ping_get_target_system(msg);
-	ping->target_component = mavlink_msg_ping_get_target_component(msg);
-#else
-	memcpy(ping, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PING_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
deleted file mode 100644
index fceb7d168..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE RADIO_STATUS PACKING
-
-#define MAVLINK_MSG_ID_RADIO_STATUS 109
-
-typedef struct __mavlink_radio_status_t
-{
- uint16_t rxerrors; ///< receive errors
- uint16_t fixed; ///< count of error corrected packets
- uint8_t rssi; ///< local signal strength
- uint8_t remrssi; ///< remote signal strength
- uint8_t txbuf; ///< how full the tx buffer is as a percentage
- uint8_t noise; ///< background noise level
- uint8_t remnoise; ///< remote background noise level
-} mavlink_radio_status_t;
-
-#define MAVLINK_MSG_ID_RADIO_STATUS_LEN 9
-#define MAVLINK_MSG_ID_109_LEN 9
-
-#define MAVLINK_MSG_ID_RADIO_STATUS_CRC 185
-#define MAVLINK_MSG_ID_109_CRC 185
-
-
-
-#define MAVLINK_MESSAGE_INFO_RADIO_STATUS { \
-	"RADIO_STATUS", \
-	7, \
-	{  { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_status_t, rxerrors) }, \
-         { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_status_t, fixed) }, \
-         { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_status_t, rssi) }, \
-         { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_status_t, remrssi) }, \
-         { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_status_t, txbuf) }, \
-         { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_status_t, noise) }, \
-         { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_status_t, remnoise) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a radio_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 rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_radio_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RADIO_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN);
-#else
-	mavlink_radio_status_t packet;
-	packet.rxerrors = rxerrors;
-	packet.fixed = fixed;
-	packet.rssi = rssi;
-	packet.remrssi = remrssi;
-	packet.txbuf = txbuf;
-	packet.noise = noise;
-	packet.remnoise = remnoise;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a radio_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 rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_radio_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RADIO_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN);
-#else
-	mavlink_radio_status_t packet;
-	packet.rxerrors = rxerrors;
-	packet.fixed = fixed;
-	packet.rssi = rssi;
-	packet.remrssi = remrssi;
-	packet.txbuf = txbuf;
-	packet.noise = noise;
-	packet.remnoise = remnoise;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a radio_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 radio_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_radio_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status)
-{
-	return mavlink_msg_radio_status_pack(system_id, component_id, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed);
-}
-
-/**
- * @brief Encode a radio_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 radio_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_radio_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status)
-{
-	return mavlink_msg_radio_status_pack_chan(system_id, component_id, chan, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed);
-}
-
-/**
- * @brief Send a radio_status message
- * @param chan MAVLink channel to send the message
- *
- * @param rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_radio_status_send(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[MAVLINK_MSG_ID_RADIO_STATUS_LEN];
-	_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;
-	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
-
-// MESSAGE RADIO_STATUS UNPACKING
-
-
-/**
- * @brief Get field rssi from radio_status message
- *
- * @return local signal strength
- */
-static inline uint8_t mavlink_msg_radio_status_get_rssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field remrssi from radio_status message
- *
- * @return remote signal strength
- */
-static inline uint8_t mavlink_msg_radio_status_get_remrssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field txbuf from radio_status message
- *
- * @return how full the tx buffer is as a percentage
- */
-static inline uint8_t mavlink_msg_radio_status_get_txbuf(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field noise from radio_status message
- *
- * @return background noise level
- */
-static inline uint8_t mavlink_msg_radio_status_get_noise(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field remnoise from radio_status message
- *
- * @return remote background noise level
- */
-static inline uint8_t mavlink_msg_radio_status_get_remnoise(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field rxerrors from radio_status message
- *
- * @return receive errors
- */
-static inline uint16_t mavlink_msg_radio_status_get_rxerrors(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field fixed from radio_status message
- *
- * @return count of error corrected packets
- */
-static inline uint16_t mavlink_msg_radio_status_get_fixed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a radio_status message into a struct
- *
- * @param msg The message to decode
- * @param radio_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_radio_status_decode(const mavlink_message_t* msg, mavlink_radio_status_t* radio_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	radio_status->rxerrors = mavlink_msg_radio_status_get_rxerrors(msg);
-	radio_status->fixed = mavlink_msg_radio_status_get_fixed(msg);
-	radio_status->rssi = mavlink_msg_radio_status_get_rssi(msg);
-	radio_status->remrssi = mavlink_msg_radio_status_get_remrssi(msg);
-	radio_status->txbuf = mavlink_msg_radio_status_get_txbuf(msg);
-	radio_status->noise = mavlink_msg_radio_status_get_noise(msg);
-	radio_status->remnoise = mavlink_msg_radio_status_get_remnoise(msg);
-#else
-	memcpy(radio_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_STATUS_LEN);
-#endif
-}
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
deleted file mode 100644
index 62a9b6eea..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE RAW_IMU PACKING
-
-#define MAVLINK_MSG_ID_RAW_IMU 27
-
-typedef struct __mavlink_raw_imu_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- int16_t xacc; ///< X acceleration (raw)
- int16_t yacc; ///< Y acceleration (raw)
- int16_t zacc; ///< Z acceleration (raw)
- int16_t xgyro; ///< Angular speed around X axis (raw)
- int16_t ygyro; ///< Angular speed around Y axis (raw)
- int16_t zgyro; ///< Angular speed around Z axis (raw)
- int16_t xmag; ///< X Magnetic field (raw)
- int16_t ymag; ///< Y Magnetic field (raw)
- int16_t zmag; ///< Z Magnetic field (raw)
-} mavlink_raw_imu_t;
-
-#define MAVLINK_MSG_ID_RAW_IMU_LEN 26
-#define MAVLINK_MSG_ID_27_LEN 26
-
-#define MAVLINK_MSG_ID_RAW_IMU_CRC 144
-#define MAVLINK_MSG_ID_27_CRC 144
-
-
-
-#define MAVLINK_MESSAGE_INFO_RAW_IMU { \
-	"RAW_IMU", \
-	10, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, time_usec) }, \
-         { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_imu_t, zacc) }, \
-         { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_imu_t, xgyro) }, \
-         { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_raw_imu_t, ygyro) }, \
-         { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_raw_imu_t, zgyro) }, \
-         { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \
-         { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \
-         { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a raw_imu 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param xacc X acceleration (raw)
- * @param yacc Y acceleration (raw)
- * @param zacc Z acceleration (raw)
- * @param xgyro Angular speed around X axis (raw)
- * @param ygyro Angular speed around Y axis (raw)
- * @param zgyro Angular speed around Z axis (raw)
- * @param xmag X Magnetic field (raw)
- * @param ymag Y Magnetic field (raw)
- * @param zmag Z Magnetic field (raw)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RAW_IMU_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN);
-#else
-	mavlink_raw_imu_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RAW_IMU;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN);
-#endif
-}
-
-/**
- * @brief Pack a raw_imu 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param xacc X acceleration (raw)
- * @param yacc Y acceleration (raw)
- * @param zacc Z acceleration (raw)
- * @param xgyro Angular speed around X axis (raw)
- * @param ygyro Angular speed around Y axis (raw)
- * @param zgyro Angular speed around Z axis (raw)
- * @param xmag X Magnetic field (raw)
- * @param ymag Y Magnetic field (raw)
- * @param zmag Z Magnetic field (raw)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RAW_IMU_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN);
-#else
-	mavlink_raw_imu_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RAW_IMU;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN);
-#endif
-}
-
-/**
- * @brief Encode a raw_imu 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 raw_imu C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu)
-{
-	return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag);
-}
-
-/**
- * @brief Encode a raw_imu 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 raw_imu C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu)
-{
-	return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag);
-}
-
-/**
- * @brief Send a raw_imu message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param xacc X acceleration (raw)
- * @param yacc Y acceleration (raw)
- * @param zacc Z acceleration (raw)
- * @param xgyro Angular speed around X axis (raw)
- * @param ygyro Angular speed around Y axis (raw)
- * @param zgyro Angular speed around Z axis (raw)
- * @param xmag X Magnetic field (raw)
- * @param ymag Y Magnetic field (raw)
- * @param zmag Z Magnetic field (raw)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_raw_imu_send(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[MAVLINK_MSG_ID_RAW_IMU_LEN];
-	_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;
-	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
-
-// MESSAGE RAW_IMU UNPACKING
-
-
-/**
- * @brief Get field time_usec from raw_imu message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_raw_imu_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field xacc from raw_imu message
- *
- * @return X acceleration (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field yacc from raw_imu message
- *
- * @return Y acceleration (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  10);
-}
-
-/**
- * @brief Get field zacc from raw_imu message
- *
- * @return Z acceleration (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Get field xgyro from raw_imu message
- *
- * @return Angular speed around X axis (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  14);
-}
-
-/**
- * @brief Get field ygyro from raw_imu message
- *
- * @return Angular speed around Y axis (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  16);
-}
-
-/**
- * @brief Get field zgyro from raw_imu message
- *
- * @return Angular speed around Z axis (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  18);
-}
-
-/**
- * @brief Get field xmag from raw_imu message
- *
- * @return X Magnetic field (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Get field ymag from raw_imu message
- *
- * @return Y Magnetic field (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  22);
-}
-
-/**
- * @brief Get field zmag from raw_imu message
- *
- * @return Z Magnetic field (raw)
- */
-static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  24);
-}
-
-/**
- * @brief Decode a raw_imu message into a struct
- *
- * @param msg The message to decode
- * @param raw_imu C-struct to decode the message contents into
- */
-static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	raw_imu->time_usec = mavlink_msg_raw_imu_get_time_usec(msg);
-	raw_imu->xacc = mavlink_msg_raw_imu_get_xacc(msg);
-	raw_imu->yacc = mavlink_msg_raw_imu_get_yacc(msg);
-	raw_imu->zacc = mavlink_msg_raw_imu_get_zacc(msg);
-	raw_imu->xgyro = mavlink_msg_raw_imu_get_xgyro(msg);
-	raw_imu->ygyro = mavlink_msg_raw_imu_get_ygyro(msg);
-	raw_imu->zgyro = mavlink_msg_raw_imu_get_zgyro(msg);
-	raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg);
-	raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg);
-	raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg);
-#else
-	memcpy(raw_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_IMU_LEN);
-#endif
-}
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
deleted file mode 100644
index 82c5fad4a..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE RAW_PRESSURE PACKING
-
-#define MAVLINK_MSG_ID_RAW_PRESSURE 28
-
-typedef struct __mavlink_raw_pressure_t
-{
- uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- int16_t press_abs; ///< Absolute pressure (raw)
- int16_t press_diff1; ///< Differential pressure 1 (raw)
- int16_t press_diff2; ///< Differential pressure 2 (raw)
- int16_t temperature; ///< Raw Temperature measurement (raw)
-} mavlink_raw_pressure_t;
-
-#define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16
-#define MAVLINK_MSG_ID_28_LEN 16
-
-#define MAVLINK_MSG_ID_RAW_PRESSURE_CRC 67
-#define MAVLINK_MSG_ID_28_CRC 67
-
-
-
-#define MAVLINK_MESSAGE_INFO_RAW_PRESSURE { \
-	"RAW_PRESSURE", \
-	5, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_pressure_t, time_usec) }, \
-         { "press_abs", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_pressure_t, press_abs) }, \
-         { "press_diff1", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_pressure_t, press_diff1) }, \
-         { "press_diff2", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_pressure_t, press_diff2) }, \
-         { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_pressure_t, temperature) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a raw_pressure 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param press_abs Absolute pressure (raw)
- * @param press_diff1 Differential pressure 1 (raw)
- * @param press_diff2 Differential pressure 2 (raw)
- * @param temperature Raw Temperature measurement (raw)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RAW_PRESSURE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
-#else
-	mavlink_raw_pressure_t packet;
-	packet.time_usec = time_usec;
-	packet.press_abs = press_abs;
-	packet.press_diff1 = press_diff1;
-	packet.press_diff2 = press_diff2;
-	packet.temperature = temperature;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a raw_pressure 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param press_abs Absolute pressure (raw)
- * @param press_diff1 Differential pressure 1 (raw)
- * @param press_diff2 Differential pressure 2 (raw)
- * @param temperature Raw Temperature measurement (raw)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RAW_PRESSURE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
-#else
-	mavlink_raw_pressure_t packet;
-	packet.time_usec = time_usec;
-	packet.press_abs = press_abs;
-	packet.press_diff1 = press_diff1;
-	packet.press_diff2 = press_diff2;
-	packet.temperature = temperature;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a raw_pressure 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 raw_pressure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure)
-{
-	return mavlink_msg_raw_pressure_pack(system_id, component_id, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature);
-}
-
-/**
- * @brief Encode a raw_pressure 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 raw_pressure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_raw_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure)
-{
-	return mavlink_msg_raw_pressure_pack_chan(system_id, component_id, chan, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature);
-}
-
-/**
- * @brief Send a raw_pressure message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- * @param press_abs Absolute pressure (raw)
- * @param press_diff1 Differential pressure 1 (raw)
- * @param press_diff2 Differential pressure 2 (raw)
- * @param temperature Raw Temperature measurement (raw)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_raw_pressure_send(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[MAVLINK_MSG_ID_RAW_PRESSURE_LEN];
-	_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;
-	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
-
-// MESSAGE RAW_PRESSURE UNPACKING
-
-
-/**
- * @brief Get field time_usec from raw_pressure message
- *
- * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- */
-static inline uint64_t mavlink_msg_raw_pressure_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field press_abs from raw_pressure message
- *
- * @return Absolute pressure (raw)
- */
-static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field press_diff1 from raw_pressure message
- *
- * @return Differential pressure 1 (raw)
- */
-static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  10);
-}
-
-/**
- * @brief Get field press_diff2 from raw_pressure message
- *
- * @return Differential pressure 2 (raw)
- */
-static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Get field temperature from raw_pressure message
- *
- * @return Raw Temperature measurement (raw)
- */
-static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  14);
-}
-
-/**
- * @brief Decode a raw_pressure message into a struct
- *
- * @param msg The message to decode
- * @param raw_pressure C-struct to decode the message contents into
- */
-static inline void mavlink_msg_raw_pressure_decode(const mavlink_message_t* msg, mavlink_raw_pressure_t* raw_pressure)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	raw_pressure->time_usec = mavlink_msg_raw_pressure_get_time_usec(msg);
-	raw_pressure->press_abs = mavlink_msg_raw_pressure_get_press_abs(msg);
-	raw_pressure->press_diff1 = mavlink_msg_raw_pressure_get_press_diff1(msg);
-	raw_pressure->press_diff2 = mavlink_msg_raw_pressure_get_press_diff2(msg);
-	raw_pressure->temperature = mavlink_msg_raw_pressure_get_temperature(msg);
-#else
-	memcpy(raw_pressure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_PRESSURE_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
deleted file mode 100644
index 0d8a1514b..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE RC_CHANNELS_OVERRIDE PACKING
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70
-
-typedef struct __mavlink_rc_channels_override_t
-{
- uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_rc_channels_override_t;
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18
-#define MAVLINK_MSG_ID_70_LEN 18
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC 124
-#define MAVLINK_MSG_ID_70_CRC 124
-
-
-
-#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \
-	"RC_CHANNELS_OVERRIDE", \
-	10, \
-	{  { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \
-         { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_override_t, chan2_raw) }, \
-         { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_override_t, chan3_raw) }, \
-         { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_override_t, chan4_raw) }, \
-         { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_override_t, chan5_raw) }, \
-         { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \
-         { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \
-         { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_override_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rc_channels_override_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a rc_channels_override 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 target_system System ID
- * @param target_component Component ID
- * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
-#else
-	mavlink_rc_channels_override_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a rc_channels_override 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 target_system System ID
- * @param target_component Component ID
- * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
-#else
-	mavlink_rc_channels_override_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a rc_channels_override 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_override C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override)
-{
-	return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw);
-}
-
-/**
- * @brief Encode a rc_channels_override 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_override C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override)
-{
-	return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw);
-}
-
-/**
- * @brief Send a rc_channels_override message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_rc_channels_override_send(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[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN];
-	_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;
-	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
-
-// MESSAGE RC_CHANNELS_OVERRIDE UNPACKING
-
-
-/**
- * @brief Get field target_system from rc_channels_override message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field target_component from rc_channels_override message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  17);
-}
-
-/**
- * @brief Get field chan1_raw from rc_channels_override message
- *
- * @return RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field chan2_raw from rc_channels_override message
- *
- * @return RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Get field chan3_raw from rc_channels_override message
- *
- * @return RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field chan4_raw from rc_channels_override message
- *
- * @return RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field chan5_raw from rc_channels_override message
- *
- * @return RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field chan6_raw from rc_channels_override message
- *
- * @return RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Get field chan7_raw from rc_channels_override message
- *
- * @return RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field chan8_raw from rc_channels_override message
- *
- * @return RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field.
- */
-static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  14);
-}
-
-/**
- * @brief Decode a rc_channels_override message into a struct
- *
- * @param msg The message to decode
- * @param rc_channels_override C-struct to decode the message contents into
- */
-static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message_t* msg, mavlink_rc_channels_override_t* rc_channels_override)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	rc_channels_override->chan1_raw = mavlink_msg_rc_channels_override_get_chan1_raw(msg);
-	rc_channels_override->chan2_raw = mavlink_msg_rc_channels_override_get_chan2_raw(msg);
-	rc_channels_override->chan3_raw = mavlink_msg_rc_channels_override_get_chan3_raw(msg);
-	rc_channels_override->chan4_raw = mavlink_msg_rc_channels_override_get_chan4_raw(msg);
-	rc_channels_override->chan5_raw = mavlink_msg_rc_channels_override_get_chan5_raw(msg);
-	rc_channels_override->chan6_raw = mavlink_msg_rc_channels_override_get_chan6_raw(msg);
-	rc_channels_override->chan7_raw = mavlink_msg_rc_channels_override_get_chan7_raw(msg);
-	rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg);
-	rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg);
-	rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg);
-#else
-	memcpy(rc_channels_override, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
-#endif
-}
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
deleted file mode 100644
index 3c0aed020..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h
+++ /dev/null
@@ -1,397 +0,0 @@
-// MESSAGE RC_CHANNELS_RAW PACKING
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35
-
-typedef struct __mavlink_rc_channels_raw_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.
- uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
-} mavlink_rc_channels_raw_t;
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 22
-#define MAVLINK_MSG_ID_35_LEN 22
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC 244
-#define MAVLINK_MSG_ID_35_CRC 244
-
-
-
-#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW { \
-	"RC_CHANNELS_RAW", \
-	11, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_raw_t, time_boot_ms) }, \
-         { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_raw_t, chan1_raw) }, \
-         { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_raw_t, chan2_raw) }, \
-         { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_raw_t, chan3_raw) }, \
-         { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_raw_t, chan4_raw) }, \
-         { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_raw_t, chan5_raw) }, \
-         { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_raw_t, chan6_raw) }, \
-         { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_raw_t, chan7_raw) }, \
-         { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_raw_t, chan8_raw) }, \
-         { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_rc_channels_raw_t, port) }, \
-         { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_rc_channels_raw_t, rssi) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a rc_channels_raw 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 port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- * @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 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_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RC_CHANNELS_RAW_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_uint8_t(buf, 20, port);
-	_mav_put_uint8_t(buf, 21, rssi);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
-#else
-	mavlink_rc_channels_raw_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.port = port;
-	packet.rssi = rssi;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
-#endif
-}
-
-/**
- * @brief Pack a rc_channels_raw 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 port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- * @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 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_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RC_CHANNELS_RAW_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_uint8_t(buf, 20, port);
-	_mav_put_uint8_t(buf, 21, rssi);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
-#else
-	mavlink_rc_channels_raw_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.port = port;
-	packet.rssi = rssi;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
-#endif
-}
-
-/**
- * @brief Encode a rc_channels_raw 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_raw C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw)
-{
-	return mavlink_msg_rc_channels_raw_pack(system_id, component_id, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi);
-}
-
-/**
- * @brief Encode a rc_channels_raw 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_raw C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rc_channels_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw)
-{
-	return mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, chan, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi);
-}
-
-/**
- * @brief Send a rc_channels_raw message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- * @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 rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_rc_channels_raw_send(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[MAVLINK_MSG_ID_RC_CHANNELS_RAW_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_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;
-	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
-
-// MESSAGE RC_CHANNELS_RAW UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from rc_channels_raw message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_rc_channels_raw_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field port from rc_channels_raw message
- *
- * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- */
-static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  20);
-}
-
-/**
- * @brief Get field chan1_raw from rc_channels_raw 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_raw_get_chan1_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field chan2_raw from rc_channels_raw 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_raw_get_chan2_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field chan3_raw from rc_channels_raw 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_raw_get_chan3_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field chan4_raw from rc_channels_raw 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_raw_get_chan4_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Get field chan5_raw from rc_channels_raw 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_raw_get_chan5_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field chan6_raw from rc_channels_raw 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_raw_get_chan6_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  14);
-}
-
-/**
- * @brief Get field chan7_raw from rc_channels_raw 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_raw_get_chan7_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field chan8_raw from rc_channels_raw 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_raw_get_chan8_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  18);
-}
-
-/**
- * @brief Get field rssi from rc_channels_raw message
- *
- * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
- */
-static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  21);
-}
-
-/**
- * @brief Decode a rc_channels_raw message into a struct
- *
- * @param msg The message to decode
- * @param rc_channels_raw C-struct to decode the message contents into
- */
-static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* msg, mavlink_rc_channels_raw_t* rc_channels_raw)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	rc_channels_raw->time_boot_ms = mavlink_msg_rc_channels_raw_get_time_boot_ms(msg);
-	rc_channels_raw->chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(msg);
-	rc_channels_raw->chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(msg);
-	rc_channels_raw->chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(msg);
-	rc_channels_raw->chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(msg);
-	rc_channels_raw->chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(msg);
-	rc_channels_raw->chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(msg);
-	rc_channels_raw->chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(msg);
-	rc_channels_raw->chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(msg);
-	rc_channels_raw->port = mavlink_msg_rc_channels_raw_get_port(msg);
-	rc_channels_raw->rssi = mavlink_msg_rc_channels_raw_get_rssi(msg);
-#else
-	memcpy(rc_channels_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
-#endif
-}
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
deleted file mode 100644
index 2153ab392..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h
+++ /dev/null
@@ -1,397 +0,0 @@
-// MESSAGE RC_CHANNELS_SCALED PACKING
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 34
-
-typedef struct __mavlink_rc_channels_scaled_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
-} mavlink_rc_channels_scaled_t;
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 22
-#define MAVLINK_MSG_ID_34_LEN 22
-
-#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC 237
-#define MAVLINK_MSG_ID_34_CRC 237
-
-
-
-#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED { \
-	"RC_CHANNELS_SCALED", \
-	11, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_scaled_t, time_boot_ms) }, \
-         { "chan1_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_rc_channels_scaled_t, chan1_scaled) }, \
-         { "chan2_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_rc_channels_scaled_t, chan2_scaled) }, \
-         { "chan3_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rc_channels_scaled_t, chan3_scaled) }, \
-         { "chan4_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rc_channels_scaled_t, chan4_scaled) }, \
-         { "chan5_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_rc_channels_scaled_t, chan5_scaled) }, \
-         { "chan6_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_rc_channels_scaled_t, chan6_scaled) }, \
-         { "chan7_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_rc_channels_scaled_t, chan7_scaled) }, \
-         { "chan8_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_rc_channels_scaled_t, chan8_scaled) }, \
-         { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_rc_channels_scaled_t, port) }, \
-         { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_rc_channels_scaled_t, rssi) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a rc_channels_scaled 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 port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @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_scaled_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
-#else
-	mavlink_rc_channels_scaled_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
-#endif
-}
-
-/**
- * @brief Pack a rc_channels_scaled 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 port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @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_scaled_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
-#else
-	mavlink_rc_channels_scaled_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
-#endif
-}
-
-/**
- * @brief Encode a rc_channels_scaled 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_scaled C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled)
-{
-	return mavlink_msg_rc_channels_scaled_pack(system_id, component_id, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi);
-}
-
-/**
- * @brief Encode a rc_channels_scaled 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_scaled C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rc_channels_scaled_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled)
-{
-	return mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, chan, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi);
-}
-
-/**
- * @brief Send a rc_channels_scaled message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- * @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_scaled_send(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[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN];
-	_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;
-	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
-
-// MESSAGE RC_CHANNELS_SCALED UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from rc_channels_scaled message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_rc_channels_scaled_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field port from rc_channels_scaled message
- *
- * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos.
- */
-static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  20);
-}
-
-/**
- * @brief Get field chan1_scaled from rc_channels_scaled message
- *
- * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- */
-static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  4);
-}
-
-/**
- * @brief Get field chan2_scaled from rc_channels_scaled message
- *
- * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- */
-static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  6);
-}
-
-/**
- * @brief Get field chan3_scaled from rc_channels_scaled message
- *
- * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- */
-static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field chan4_scaled from rc_channels_scaled message
- *
- * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- */
-static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  10);
-}
-
-/**
- * @brief Get field chan5_scaled from rc_channels_scaled message
- *
- * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- */
-static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Get field chan6_scaled from rc_channels_scaled message
- *
- * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- */
-static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  14);
-}
-
-/**
- * @brief Get field chan7_scaled from rc_channels_scaled message
- *
- * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- */
-static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  16);
-}
-
-/**
- * @brief Get field chan8_scaled from rc_channels_scaled message
- *
- * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
- */
-static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  18);
-}
-
-/**
- * @brief Get field rssi from rc_channels_scaled message
- *
- * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
- */
-static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  21);
-}
-
-/**
- * @brief Decode a rc_channels_scaled message into a struct
- *
- * @param msg The message to decode
- * @param rc_channels_scaled C-struct to decode the message contents into
- */
-static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t* msg, mavlink_rc_channels_scaled_t* rc_channels_scaled)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	rc_channels_scaled->time_boot_ms = mavlink_msg_rc_channels_scaled_get_time_boot_ms(msg);
-	rc_channels_scaled->chan1_scaled = mavlink_msg_rc_channels_scaled_get_chan1_scaled(msg);
-	rc_channels_scaled->chan2_scaled = mavlink_msg_rc_channels_scaled_get_chan2_scaled(msg);
-	rc_channels_scaled->chan3_scaled = mavlink_msg_rc_channels_scaled_get_chan3_scaled(msg);
-	rc_channels_scaled->chan4_scaled = mavlink_msg_rc_channels_scaled_get_chan4_scaled(msg);
-	rc_channels_scaled->chan5_scaled = mavlink_msg_rc_channels_scaled_get_chan5_scaled(msg);
-	rc_channels_scaled->chan6_scaled = mavlink_msg_rc_channels_scaled_get_chan6_scaled(msg);
-	rc_channels_scaled->chan7_scaled = mavlink_msg_rc_channels_scaled_get_chan7_scaled(msg);
-	rc_channels_scaled->chan8_scaled = mavlink_msg_rc_channels_scaled_get_chan8_scaled(msg);
-	rc_channels_scaled->port = mavlink_msg_rc_channels_scaled_get_port(msg);
-	rc_channels_scaled->rssi = mavlink_msg_rc_channels_scaled_get_rssi(msg);
-#else
-	memcpy(rc_channels_scaled, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
-#endif
-}
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
deleted file mode 100644
index c754ad876..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE REQUEST_DATA_STREAM PACKING
-
-#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66
-
-typedef struct __mavlink_request_data_stream_t
-{
- uint16_t req_message_rate; ///< The requested interval between two messages of this type
- uint8_t target_system; ///< The target requested to send the message stream.
- uint8_t target_component; ///< The target requested to send the message stream.
- uint8_t req_stream_id; ///< The ID of the requested data stream
- uint8_t start_stop; ///< 1 to start sending, 0 to stop sending.
-} mavlink_request_data_stream_t;
-
-#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6
-#define MAVLINK_MSG_ID_66_LEN 6
-
-#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC 148
-#define MAVLINK_MSG_ID_66_CRC 148
-
-
-
-#define MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM { \
-	"REQUEST_DATA_STREAM", \
-	5, \
-	{  { "req_message_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_request_data_stream_t, req_message_rate) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_request_data_stream_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_request_data_stream_t, target_component) }, \
-         { "req_stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_request_data_stream_t, req_stream_id) }, \
-         { "start_stop", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_request_data_stream_t, start_stop) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a request_data_stream 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 target_system The target requested to send the message stream.
- * @param target_component The target requested to send the message stream.
- * @param req_stream_id The ID of the requested data stream
- * @param req_message_rate The requested interval between two messages of this type
- * @param start_stop 1 to start sending, 0 to stop sending.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
-#else
-	mavlink_request_data_stream_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
-#endif
-}
-
-/**
- * @brief Pack a request_data_stream 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 target_system The target requested to send the message stream.
- * @param target_component The target requested to send the message stream.
- * @param req_stream_id The ID of the requested data stream
- * @param req_message_rate The requested interval between two messages of this type
- * @param start_stop 1 to start sending, 0 to stop sending.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
-#else
-	mavlink_request_data_stream_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
-#endif
-}
-
-/**
- * @brief Encode a request_data_stream 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 request_data_stream C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream)
-{
-	return mavlink_msg_request_data_stream_pack(system_id, component_id, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop);
-}
-
-/**
- * @brief Encode a request_data_stream 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 request_data_stream C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_request_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream)
-{
-	return mavlink_msg_request_data_stream_pack_chan(system_id, component_id, chan, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop);
-}
-
-/**
- * @brief Send a request_data_stream message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system The target requested to send the message stream.
- * @param target_component The target requested to send the message stream.
- * @param req_stream_id The ID of the requested data stream
- * @param req_message_rate The requested interval between two messages of this type
- * @param start_stop 1 to start sending, 0 to stop sending.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_request_data_stream_send(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[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN];
-	_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;
-	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
-
-// MESSAGE REQUEST_DATA_STREAM UNPACKING
-
-
-/**
- * @brief Get field target_system from request_data_stream message
- *
- * @return The target requested to send the message stream.
- */
-static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field target_component from request_data_stream message
- *
- * @return The target requested to send the message stream.
- */
-static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field req_stream_id from request_data_stream message
- *
- * @return The ID of the requested data stream
- */
-static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field req_message_rate from request_data_stream message
- *
- * @return The requested interval between two messages of this type
- */
-static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field start_stop from request_data_stream message
- *
- * @return 1 to start sending, 0 to stop sending.
- */
-static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Decode a request_data_stream message into a struct
- *
- * @param msg The message to decode
- * @param request_data_stream C-struct to decode the message contents into
- */
-static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_t* msg, mavlink_request_data_stream_t* request_data_stream)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	request_data_stream->req_message_rate = mavlink_msg_request_data_stream_get_req_message_rate(msg);
-	request_data_stream->target_system = mavlink_msg_request_data_stream_get_target_system(msg);
-	request_data_stream->target_component = mavlink_msg_request_data_stream_get_target_component(msg);
-	request_data_stream->req_stream_id = mavlink_msg_request_data_stream_get_req_stream_id(msg);
-	request_data_stream->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg);
-#else
-	memcpy(request_data_stream, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
-#endif
-}
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
deleted file mode 100644
index ac3ef4fa9..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT 80
-
-typedef struct __mavlink_roll_pitch_yaw_rates_thrust_setpoint_t
-{
- uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot
- float roll_rate; ///< Desired roll rate in radians per second
- float pitch_rate; ///< Desired pitch rate in radians per second
- float yaw_rate; ///< Desired yaw rate in radians per second
- float thrust; ///< Collective thrust, normalized to 0 .. 1
-} mavlink_roll_pitch_yaw_rates_thrust_setpoint_t;
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN 20
-#define MAVLINK_MSG_ID_80_LEN 20
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC 127
-#define MAVLINK_MSG_ID_80_CRC 127
-
-
-
-#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT { \
-	"ROLL_PITCH_YAW_RATES_THRUST_SETPOINT", \
-	5, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, time_boot_ms) }, \
-         { "roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, roll_rate) }, \
-         { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, pitch_rate) }, \
-         { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, yaw_rate) }, \
-         { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t, thrust) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint 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 in milliseconds since system boot
- * @param roll_rate Desired roll rate in radians per second
- * @param pitch_rate Desired pitch rate in radians per second
- * @param yaw_rate Desired yaw rate in radians per second
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
-#else
-	mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint 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 in milliseconds since system boot
- * @param roll_rate Desired roll rate in radians per second
- * @param pitch_rate Desired pitch rate in radians per second
- * @param yaw_rate Desired yaw rate in radians per second
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
-#else
-	mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint 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 roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint)
-{
-	return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust);
-}
-
-/**
- * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint 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 roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint)
-{
-	return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust);
-}
-
-/**
- * @brief Send a roll_pitch_yaw_rates_thrust_setpoint message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp in milliseconds since system boot
- * @param roll_rate Desired roll rate in radians per second
- * @param pitch_rate Desired pitch rate in radians per second
- * @param yaw_rate Desired yaw rate in radians per second
- * @param thrust Collective thrust, normalized to 0 .. 1
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(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[MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN];
-	_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;
-	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
-
-// MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from roll_pitch_yaw_rates_thrust_setpoint message
- *
- * @return Timestamp in milliseconds since system boot
- */
-static inline uint32_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field roll_rate from roll_pitch_yaw_rates_thrust_setpoint message
- *
- * @return Desired roll rate in radians per second
- */
-static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_roll_rate(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field pitch_rate from roll_pitch_yaw_rates_thrust_setpoint message
- *
- * @return Desired pitch rate in radians per second
- */
-static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_pitch_rate(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw_rate from roll_pitch_yaw_rates_thrust_setpoint message
- *
- * @return Desired yaw rate in radians per second
- */
-static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_yaw_rate(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field thrust from roll_pitch_yaw_rates_thrust_setpoint message
- *
- * @return Collective thrust, normalized to 0 .. 1
- */
-static inline float mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_thrust(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a roll_pitch_yaw_rates_thrust_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_time_boot_ms(msg);
-	roll_pitch_yaw_rates_thrust_setpoint->roll_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_roll_rate(msg);
-	roll_pitch_yaw_rates_thrust_setpoint->pitch_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_pitch_rate(msg);
-	roll_pitch_yaw_rates_thrust_setpoint->yaw_rate = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_yaw_rate(msg);
-	roll_pitch_yaw_rates_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_get_thrust(msg);
-#else
-	memcpy(roll_pitch_yaw_rates_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
-#endif
-}
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
deleted file mode 100644
index 626477322..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 59
-
-typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t
-{
- uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot
- float roll_speed; ///< Desired roll angular speed in rad/s
- float pitch_speed; ///< Desired pitch angular speed in rad/s
- float yaw_speed; ///< Desired yaw angular speed in rad/s
- float thrust; ///< Collective thrust, normalized to 0 .. 1
-} mavlink_roll_pitch_yaw_speed_thrust_setpoint_t;
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN 20
-#define MAVLINK_MSG_ID_59_LEN 20
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC 238
-#define MAVLINK_MSG_ID_59_CRC 238
-
-
-
-#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT { \
-	"ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT", \
-	5, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, time_boot_ms) }, \
-         { "roll_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, roll_speed) }, \
-         { "pitch_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, pitch_speed) }, \
-         { "yaw_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, yaw_speed) }, \
-         { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, thrust) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint 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 in milliseconds since system boot
- * @param roll_speed Desired roll angular speed in rad/s
- * @param pitch_speed Desired pitch angular speed in rad/s
- * @param yaw_speed Desired yaw angular speed in rad/s
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
-#else
-	mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint 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 in milliseconds since system boot
- * @param roll_speed Desired roll angular speed in rad/s
- * @param pitch_speed Desired pitch angular speed in rad/s
- * @param yaw_speed Desired yaw angular speed in rad/s
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
-#else
-	mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint 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 roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint)
-{
-	return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust);
-}
-
-/**
- * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint 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 roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint)
-{
-	return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust);
-}
-
-/**
- * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp in milliseconds since system boot
- * @param roll_speed Desired roll angular speed in rad/s
- * @param pitch_speed Desired pitch angular speed in rad/s
- * @param yaw_speed Desired yaw angular speed in rad/s
- * @param thrust Collective thrust, normalized to 0 .. 1
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(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[MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN];
-	_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;
-	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
-
-// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from roll_pitch_yaw_speed_thrust_setpoint message
- *
- * @return Timestamp in milliseconds since system boot
- */
-static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field roll_speed from roll_pitch_yaw_speed_thrust_setpoint message
- *
- * @return Desired roll angular speed in rad/s
- */
-static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field pitch_speed from roll_pitch_yaw_speed_thrust_setpoint message
- *
- * @return Desired pitch angular speed in rad/s
- */
-static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw_speed from roll_pitch_yaw_speed_thrust_setpoint message
- *
- * @return Desired yaw angular speed in rad/s
- */
-static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field thrust from roll_pitch_yaw_speed_thrust_setpoint message
- *
- * @return Collective thrust, normalized to 0 .. 1
- */
-static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a roll_pitch_yaw_speed_thrust_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_boot_ms(msg);
-	roll_pitch_yaw_speed_thrust_setpoint->roll_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(msg);
-	roll_pitch_yaw_speed_thrust_setpoint->pitch_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(msg);
-	roll_pitch_yaw_speed_thrust_setpoint->yaw_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(msg);
-	roll_pitch_yaw_speed_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(msg);
-#else
-	memcpy(roll_pitch_yaw_speed_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
-#endif
-}
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
deleted file mode 100644
index ffcdc547b..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 58
-
-typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t
-{
- uint32_t time_boot_ms; ///< Timestamp in milliseconds since system boot
- float roll; ///< Desired roll angle in radians
- float pitch; ///< Desired pitch angle in radians
- float yaw; ///< Desired yaw angle in radians
- float thrust; ///< Collective thrust, normalized to 0 .. 1
-} mavlink_roll_pitch_yaw_thrust_setpoint_t;
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN 20
-#define MAVLINK_MSG_ID_58_LEN 20
-
-#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC 239
-#define MAVLINK_MSG_ID_58_CRC 239
-
-
-
-#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT { \
-	"ROLL_PITCH_YAW_THRUST_SETPOINT", \
-	5, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, time_boot_ms) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, yaw) }, \
-         { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, thrust) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a roll_pitch_yaw_thrust_setpoint 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 in milliseconds since system boot
- * @param roll Desired roll angle in radians
- * @param pitch Desired pitch angle in radians
- * @param yaw Desired yaw angle in radians
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
-#else
-	mavlink_roll_pitch_yaw_thrust_setpoint_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.thrust = thrust;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a roll_pitch_yaw_thrust_setpoint 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 in milliseconds since system boot
- * @param roll Desired roll angle in radians
- * @param pitch Desired pitch angle in radians
- * @param yaw Desired yaw angle in radians
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint32_t time_boot_ms,float roll,float pitch,float yaw,float thrust)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
-#else
-	mavlink_roll_pitch_yaw_thrust_setpoint_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.thrust = thrust;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a roll_pitch_yaw_thrust_setpoint 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 roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint)
-{
-	return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust);
-}
-
-/**
- * @brief Encode a roll_pitch_yaw_thrust_setpoint 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 roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint)
-{
-	return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust);
-}
-
-/**
- * @brief Send a roll_pitch_yaw_thrust_setpoint message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp in milliseconds since system boot
- * @param roll Desired roll angle in radians
- * @param pitch Desired pitch angle in radians
- * @param yaw Desired yaw angle in radians
- * @param thrust Collective thrust, normalized to 0 .. 1
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(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[MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN];
-	_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;
-	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
-
-// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from roll_pitch_yaw_thrust_setpoint message
- *
- * @return Timestamp in milliseconds since system boot
- */
-static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field roll from roll_pitch_yaw_thrust_setpoint message
- *
- * @return Desired roll angle in radians
- */
-static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field pitch from roll_pitch_yaw_thrust_setpoint message
- *
- * @return Desired pitch angle in radians
- */
-static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw from roll_pitch_yaw_thrust_setpoint message
- *
- * @return Desired yaw angle in radians
- */
-static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field thrust from roll_pitch_yaw_thrust_setpoint message
- *
- * @return Collective thrust, normalized to 0 .. 1
- */
-static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a roll_pitch_yaw_thrust_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param roll_pitch_yaw_thrust_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	roll_pitch_yaw_thrust_setpoint->time_boot_ms = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ms(msg);
-	roll_pitch_yaw_thrust_setpoint->roll = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(msg);
-	roll_pitch_yaw_thrust_setpoint->pitch = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(msg);
-	roll_pitch_yaw_thrust_setpoint->yaw = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(msg);
-	roll_pitch_yaw_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(msg);
-#else
-	memcpy(roll_pitch_yaw_thrust_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
-#endif
-}
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
deleted file mode 100644
index fcd54cbb7..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE SAFETY_ALLOWED_AREA PACKING
-
-#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 55
-
-typedef struct __mavlink_safety_allowed_area_t
-{
- float p1x; ///< x position 1 / Latitude 1
- float p1y; ///< y position 1 / Longitude 1
- float p1z; ///< z position 1 / Altitude 1
- float p2x; ///< x position 2 / Latitude 2
- float p2y; ///< y position 2 / Longitude 2
- float p2z; ///< z position 2 / Altitude 2
- uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
-} mavlink_safety_allowed_area_t;
-
-#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25
-#define MAVLINK_MSG_ID_55_LEN 25
-
-#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC 3
-#define MAVLINK_MSG_ID_55_CRC 3
-
-
-
-#define MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA { \
-	"SAFETY_ALLOWED_AREA", \
-	7, \
-	{  { "p1x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_safety_allowed_area_t, p1x) }, \
-         { "p1y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_safety_allowed_area_t, p1y) }, \
-         { "p1z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_safety_allowed_area_t, p1z) }, \
-         { "p2x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_safety_allowed_area_t, p2x) }, \
-         { "p2y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_safety_allowed_area_t, p2y) }, \
-         { "p2z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_safety_allowed_area_t, p2z) }, \
-         { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_safety_allowed_area_t, frame) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a safety_allowed_area 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 frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
- * @param p1x x position 1 / Latitude 1
- * @param p1y y position 1 / Longitude 1
- * @param p1z z position 1 / Altitude 1
- * @param p2x x position 2 / Latitude 2
- * @param p2y y position 2 / Longitude 2
- * @param p2z z position 2 / Altitude 2
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
-#else
-	mavlink_safety_allowed_area_t packet;
-	packet.p1x = p1x;
-	packet.p1y = p1y;
-	packet.p1z = p1z;
-	packet.p2x = p2x;
-	packet.p2y = p2y;
-	packet.p2z = p2z;
-	packet.frame = frame;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
-#endif
-}
-
-/**
- * @brief Pack a safety_allowed_area 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 frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
- * @param p1x x position 1 / Latitude 1
- * @param p1y y position 1 / Longitude 1
- * @param p1z z position 1 / Altitude 1
- * @param p2x x position 2 / Latitude 2
- * @param p2y y position 2 / Longitude 2
- * @param p2z z position 2 / Altitude 2
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
-#else
-	mavlink_safety_allowed_area_t packet;
-	packet.p1x = p1x;
-	packet.p1y = p1y;
-	packet.p1z = p1z;
-	packet.p2x = p2x;
-	packet.p2y = p2y;
-	packet.p2z = p2z;
-	packet.frame = frame;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
-#endif
-}
-
-/**
- * @brief Encode a safety_allowed_area 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 safety_allowed_area C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area)
-{
-	return mavlink_msg_safety_allowed_area_pack(system_id, component_id, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z);
-}
-
-/**
- * @brief Encode a safety_allowed_area 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 safety_allowed_area C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_safety_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area)
-{
-	return mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z);
-}
-
-/**
- * @brief Send a safety_allowed_area message
- * @param chan MAVLink channel to send the message
- *
- * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
- * @param p1x x position 1 / Latitude 1
- * @param p1y y position 1 / Longitude 1
- * @param p1z z position 1 / Altitude 1
- * @param p2x x position 2 / Latitude 2
- * @param p2y y position 2 / Longitude 2
- * @param p2z z position 2 / Altitude 2
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_safety_allowed_area_send(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[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN];
-	_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;
-	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
-
-// MESSAGE SAFETY_ALLOWED_AREA UNPACKING
-
-
-/**
- * @brief Get field frame from safety_allowed_area message
- *
- * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
- */
-static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  24);
-}
-
-/**
- * @brief Get field p1x from safety_allowed_area message
- *
- * @return x position 1 / Latitude 1
- */
-static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field p1y from safety_allowed_area message
- *
- * @return y position 1 / Longitude 1
- */
-static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field p1z from safety_allowed_area message
- *
- * @return z position 1 / Altitude 1
- */
-static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field p2x from safety_allowed_area message
- *
- * @return x position 2 / Latitude 2
- */
-static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field p2y from safety_allowed_area message
- *
- * @return y position 2 / Longitude 2
- */
-static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field p2z from safety_allowed_area message
- *
- * @return z position 2 / Altitude 2
- */
-static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Decode a safety_allowed_area message into a struct
- *
- * @param msg The message to decode
- * @param safety_allowed_area C-struct to decode the message contents into
- */
-static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_allowed_area_t* safety_allowed_area)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	safety_allowed_area->p1x = mavlink_msg_safety_allowed_area_get_p1x(msg);
-	safety_allowed_area->p1y = mavlink_msg_safety_allowed_area_get_p1y(msg);
-	safety_allowed_area->p1z = mavlink_msg_safety_allowed_area_get_p1z(msg);
-	safety_allowed_area->p2x = mavlink_msg_safety_allowed_area_get_p2x(msg);
-	safety_allowed_area->p2y = mavlink_msg_safety_allowed_area_get_p2y(msg);
-	safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg);
-	safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg);
-#else
-	memcpy(safety_allowed_area, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
-#endif
-}
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
deleted file mode 100644
index 61f6af155..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h
+++ /dev/null
@@ -1,353 +0,0 @@
-// MESSAGE SAFETY_SET_ALLOWED_AREA PACKING
-
-#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 54
-
-typedef struct __mavlink_safety_set_allowed_area_t
-{
- float p1x; ///< x position 1 / Latitude 1
- float p1y; ///< y position 1 / Longitude 1
- float p1z; ///< z position 1 / Altitude 1
- float p2x; ///< x position 2 / Latitude 2
- float p2y; ///< y position 2 / Longitude 2
- float p2z; ///< z position 2 / Altitude 2
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
-} mavlink_safety_set_allowed_area_t;
-
-#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27
-#define MAVLINK_MSG_ID_54_LEN 27
-
-#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC 15
-#define MAVLINK_MSG_ID_54_CRC 15
-
-
-
-#define MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA { \
-	"SAFETY_SET_ALLOWED_AREA", \
-	9, \
-	{  { "p1x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_safety_set_allowed_area_t, p1x) }, \
-         { "p1y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_safety_set_allowed_area_t, p1y) }, \
-         { "p1z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_safety_set_allowed_area_t, p1z) }, \
-         { "p2x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_safety_set_allowed_area_t, p2x) }, \
-         { "p2y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_safety_set_allowed_area_t, p2y) }, \
-         { "p2z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_safety_set_allowed_area_t, p2z) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_safety_set_allowed_area_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_safety_set_allowed_area_t, target_component) }, \
-         { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_safety_set_allowed_area_t, frame) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a safety_set_allowed_area 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 target_system System ID
- * @param target_component Component ID
- * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
- * @param p1x x position 1 / Latitude 1
- * @param p1y y position 1 / Longitude 1
- * @param p1z z position 1 / Altitude 1
- * @param p2x x position 2 / Latitude 2
- * @param p2y y position 2 / Longitude 2
- * @param p2z z position 2 / Altitude 2
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
-#else
-	mavlink_safety_set_allowed_area_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
-#endif
-}
-
-/**
- * @brief Pack a safety_set_allowed_area 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 target_system System ID
- * @param target_component Component ID
- * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
- * @param p1x x position 1 / Latitude 1
- * @param p1y y position 1 / Longitude 1
- * @param p1z z position 1 / Altitude 1
- * @param p2x x position 2 / Latitude 2
- * @param p2y y position 2 / Longitude 2
- * @param p2z z position 2 / Altitude 2
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
-#else
-	mavlink_safety_set_allowed_area_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
-#endif
-}
-
-/**
- * @brief Encode a safety_set_allowed_area 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 safety_set_allowed_area C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area)
-{
-	return mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z);
-}
-
-/**
- * @brief Encode a safety_set_allowed_area 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 safety_set_allowed_area C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_safety_set_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area)
-{
-	return mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z);
-}
-
-/**
- * @brief Send a safety_set_allowed_area message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
- * @param p1x x position 1 / Latitude 1
- * @param p1y y position 1 / Longitude 1
- * @param p1z z position 1 / Altitude 1
- * @param p2x x position 2 / Latitude 2
- * @param p2y y position 2 / Longitude 2
- * @param p2z z position 2 / Altitude 2
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_safety_set_allowed_area_send(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[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN];
-	_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;
-	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
-
-// MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING
-
-
-/**
- * @brief Get field target_system from safety_set_allowed_area message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  24);
-}
-
-/**
- * @brief Get field target_component from safety_set_allowed_area message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  25);
-}
-
-/**
- * @brief Get field frame from safety_set_allowed_area message
- *
- * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
- */
-static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  26);
-}
-
-/**
- * @brief Get field p1x from safety_set_allowed_area message
- *
- * @return x position 1 / Latitude 1
- */
-static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field p1y from safety_set_allowed_area message
- *
- * @return y position 1 / Longitude 1
- */
-static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field p1z from safety_set_allowed_area message
- *
- * @return z position 1 / Altitude 1
- */
-static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field p2x from safety_set_allowed_area message
- *
- * @return x position 2 / Latitude 2
- */
-static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field p2y from safety_set_allowed_area message
- *
- * @return y position 2 / Longitude 2
- */
-static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field p2z from safety_set_allowed_area message
- *
- * @return z position 2 / Altitude 2
- */
-static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Decode a safety_set_allowed_area message into a struct
- *
- * @param msg The message to decode
- * @param safety_set_allowed_area C-struct to decode the message contents into
- */
-static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_set_allowed_area_t* safety_set_allowed_area)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	safety_set_allowed_area->p1x = mavlink_msg_safety_set_allowed_area_get_p1x(msg);
-	safety_set_allowed_area->p1y = mavlink_msg_safety_set_allowed_area_get_p1y(msg);
-	safety_set_allowed_area->p1z = mavlink_msg_safety_set_allowed_area_get_p1z(msg);
-	safety_set_allowed_area->p2x = mavlink_msg_safety_set_allowed_area_get_p2x(msg);
-	safety_set_allowed_area->p2y = mavlink_msg_safety_set_allowed_area_get_p2y(msg);
-	safety_set_allowed_area->p2z = mavlink_msg_safety_set_allowed_area_get_p2z(msg);
-	safety_set_allowed_area->target_system = mavlink_msg_safety_set_allowed_area_get_target_system(msg);
-	safety_set_allowed_area->target_component = mavlink_msg_safety_set_allowed_area_get_target_component(msg);
-	safety_set_allowed_area->frame = mavlink_msg_safety_set_allowed_area_get_frame(msg);
-#else
-	memcpy(safety_set_allowed_area, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
-#endif
-}
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
deleted file mode 100644
index 3010d051a..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE SCALED_IMU PACKING
-
-#define MAVLINK_MSG_ID_SCALED_IMU 26
-
-typedef struct __mavlink_scaled_imu_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- int16_t xacc; ///< X acceleration (mg)
- int16_t yacc; ///< Y acceleration (mg)
- int16_t zacc; ///< Z acceleration (mg)
- int16_t xgyro; ///< Angular speed around X axis (millirad /sec)
- int16_t ygyro; ///< Angular speed around Y axis (millirad /sec)
- int16_t zgyro; ///< Angular speed around Z axis (millirad /sec)
- int16_t xmag; ///< X Magnetic field (milli tesla)
- int16_t ymag; ///< Y Magnetic field (milli tesla)
- int16_t zmag; ///< Z Magnetic field (milli tesla)
-} mavlink_scaled_imu_t;
-
-#define MAVLINK_MSG_ID_SCALED_IMU_LEN 22
-#define MAVLINK_MSG_ID_26_LEN 22
-
-#define MAVLINK_MSG_ID_SCALED_IMU_CRC 170
-#define MAVLINK_MSG_ID_26_CRC 170
-
-
-
-#define MAVLINK_MESSAGE_INFO_SCALED_IMU { \
-	"SCALED_IMU", \
-	10, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu_t, time_boot_ms) }, \
-         { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu_t, zacc) }, \
-         { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu_t, xgyro) }, \
-         { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu_t, ygyro) }, \
-         { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu_t, zgyro) }, \
-         { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, xmag) }, \
-         { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, ymag) }, \
-         { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, zmag) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a scaled_imu 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 xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @param xgyro Angular speed around X axis (millirad /sec)
- * @param ygyro Angular speed around Y axis (millirad /sec)
- * @param zgyro Angular speed around Z axis (millirad /sec)
- * @param xmag X Magnetic field (milli tesla)
- * @param ymag Y Magnetic field (milli tesla)
- * @param zmag Z Magnetic field (milli tesla)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SCALED_IMU_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN);
-#else
-	mavlink_scaled_imu_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SCALED_IMU;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN);
-#endif
-}
-
-/**
- * @brief Pack a scaled_imu 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 xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @param xgyro Angular speed around X axis (millirad /sec)
- * @param ygyro Angular speed around Y axis (millirad /sec)
- * @param zgyro Angular speed around Z axis (millirad /sec)
- * @param xmag X Magnetic field (milli tesla)
- * @param ymag Y Magnetic field (milli tesla)
- * @param zmag Z Magnetic field (milli tesla)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SCALED_IMU_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN);
-#else
-	mavlink_scaled_imu_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SCALED_IMU;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN);
-#endif
-}
-
-/**
- * @brief Encode a scaled_imu 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 scaled_imu C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu)
-{
-	return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag);
-}
-
-/**
- * @brief Encode a scaled_imu 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 scaled_imu C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu)
-{
-	return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag);
-}
-
-/**
- * @brief Send a scaled_imu message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @param xgyro Angular speed around X axis (millirad /sec)
- * @param ygyro Angular speed around Y axis (millirad /sec)
- * @param zgyro Angular speed around Z axis (millirad /sec)
- * @param xmag X Magnetic field (milli tesla)
- * @param ymag Y Magnetic field (milli tesla)
- * @param zmag Z Magnetic field (milli tesla)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_scaled_imu_send(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[MAVLINK_MSG_ID_SCALED_IMU_LEN];
-	_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;
-	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
-
-// MESSAGE SCALED_IMU UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from scaled_imu message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_scaled_imu_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field xacc from scaled_imu message
- *
- * @return X acceleration (mg)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  4);
-}
-
-/**
- * @brief Get field yacc from scaled_imu message
- *
- * @return Y acceleration (mg)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  6);
-}
-
-/**
- * @brief Get field zacc from scaled_imu message
- *
- * @return Z acceleration (mg)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field xgyro from scaled_imu message
- *
- * @return Angular speed around X axis (millirad /sec)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  10);
-}
-
-/**
- * @brief Get field ygyro from scaled_imu message
- *
- * @return Angular speed around Y axis (millirad /sec)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Get field zgyro from scaled_imu message
- *
- * @return Angular speed around Z axis (millirad /sec)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  14);
-}
-
-/**
- * @brief Get field xmag from scaled_imu message
- *
- * @return X Magnetic field (milli tesla)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  16);
-}
-
-/**
- * @brief Get field ymag from scaled_imu message
- *
- * @return Y Magnetic field (milli tesla)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  18);
-}
-
-/**
- * @brief Get field zmag from scaled_imu message
- *
- * @return Z Magnetic field (milli tesla)
- */
-static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Decode a scaled_imu message into a struct
- *
- * @param msg The message to decode
- * @param scaled_imu C-struct to decode the message contents into
- */
-static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, mavlink_scaled_imu_t* scaled_imu)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	scaled_imu->time_boot_ms = mavlink_msg_scaled_imu_get_time_boot_ms(msg);
-	scaled_imu->xacc = mavlink_msg_scaled_imu_get_xacc(msg);
-	scaled_imu->yacc = mavlink_msg_scaled_imu_get_yacc(msg);
-	scaled_imu->zacc = mavlink_msg_scaled_imu_get_zacc(msg);
-	scaled_imu->xgyro = mavlink_msg_scaled_imu_get_xgyro(msg);
-	scaled_imu->ygyro = mavlink_msg_scaled_imu_get_ygyro(msg);
-	scaled_imu->zgyro = mavlink_msg_scaled_imu_get_zgyro(msg);
-	scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg);
-	scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg);
-	scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg);
-#else
-	memcpy(scaled_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU_LEN);
-#endif
-}
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
deleted file mode 100644
index ea4dbbf81..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE SCALED_IMU2 PACKING
-
-#define MAVLINK_MSG_ID_SCALED_IMU2 116
-
-typedef struct __mavlink_scaled_imu2_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- int16_t xacc; ///< X acceleration (mg)
- int16_t yacc; ///< Y acceleration (mg)
- int16_t zacc; ///< Z acceleration (mg)
- int16_t xgyro; ///< Angular speed around X axis (millirad /sec)
- int16_t ygyro; ///< Angular speed around Y axis (millirad /sec)
- int16_t zgyro; ///< Angular speed around Z axis (millirad /sec)
- int16_t xmag; ///< X Magnetic field (milli tesla)
- int16_t ymag; ///< Y Magnetic field (milli tesla)
- int16_t zmag; ///< Z Magnetic field (milli tesla)
-} mavlink_scaled_imu2_t;
-
-#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 22
-#define MAVLINK_MSG_ID_116_LEN 22
-
-#define MAVLINK_MSG_ID_SCALED_IMU2_CRC 76
-#define MAVLINK_MSG_ID_116_CRC 76
-
-
-
-#define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \
-	"SCALED_IMU2", \
-	10, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \
-         { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu2_t, zacc) }, \
-         { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu2_t, xgyro) }, \
-         { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu2_t, ygyro) }, \
-         { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu2_t, zgyro) }, \
-         { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \
-         { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \
-         { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a scaled_imu2 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 xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @param xgyro Angular speed around X axis (millirad /sec)
- * @param ygyro Angular speed around Y axis (millirad /sec)
- * @param zgyro Angular speed around Z axis (millirad /sec)
- * @param xmag X Magnetic field (milli tesla)
- * @param ymag Y Magnetic field (milli tesla)
- * @param zmag Z Magnetic field (milli tesla)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SCALED_IMU2_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
-#else
-	mavlink_scaled_imu2_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
-#endif
-}
-
-/**
- * @brief Pack a scaled_imu2 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 xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @param xgyro Angular speed around X axis (millirad /sec)
- * @param ygyro Angular speed around Y axis (millirad /sec)
- * @param zgyro Angular speed around Z axis (millirad /sec)
- * @param xmag X Magnetic field (milli tesla)
- * @param ymag Y Magnetic field (milli tesla)
- * @param zmag Z Magnetic field (milli tesla)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SCALED_IMU2_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
-#else
-	mavlink_scaled_imu2_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
-#endif
-}
-
-/**
- * @brief Encode a scaled_imu2 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 scaled_imu2 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2)
-{
-	return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag);
-}
-
-/**
- * @brief Encode a scaled_imu2 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 scaled_imu2 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2)
-{
-	return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag);
-}
-
-/**
- * @brief Send a scaled_imu2 message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param xacc X acceleration (mg)
- * @param yacc Y acceleration (mg)
- * @param zacc Z acceleration (mg)
- * @param xgyro Angular speed around X axis (millirad /sec)
- * @param ygyro Angular speed around Y axis (millirad /sec)
- * @param zgyro Angular speed around Z axis (millirad /sec)
- * @param xmag X Magnetic field (milli tesla)
- * @param ymag Y Magnetic field (milli tesla)
- * @param zmag Z Magnetic field (milli tesla)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_scaled_imu2_send(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[MAVLINK_MSG_ID_SCALED_IMU2_LEN];
-	_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;
-	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
-
-// MESSAGE SCALED_IMU2 UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from scaled_imu2 message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_scaled_imu2_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field xacc from scaled_imu2 message
- *
- * @return X acceleration (mg)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  4);
-}
-
-/**
- * @brief Get field yacc from scaled_imu2 message
- *
- * @return Y acceleration (mg)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  6);
-}
-
-/**
- * @brief Get field zacc from scaled_imu2 message
- *
- * @return Z acceleration (mg)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field xgyro from scaled_imu2 message
- *
- * @return Angular speed around X axis (millirad /sec)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_xgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  10);
-}
-
-/**
- * @brief Get field ygyro from scaled_imu2 message
- *
- * @return Angular speed around Y axis (millirad /sec)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_ygyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Get field zgyro from scaled_imu2 message
- *
- * @return Angular speed around Z axis (millirad /sec)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_zgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  14);
-}
-
-/**
- * @brief Get field xmag from scaled_imu2 message
- *
- * @return X Magnetic field (milli tesla)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_xmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  16);
-}
-
-/**
- * @brief Get field ymag from scaled_imu2 message
- *
- * @return Y Magnetic field (milli tesla)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_ymag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  18);
-}
-
-/**
- * @brief Get field zmag from scaled_imu2 message
- *
- * @return Z Magnetic field (milli tesla)
- */
-static inline int16_t mavlink_msg_scaled_imu2_get_zmag(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Decode a scaled_imu2 message into a struct
- *
- * @param msg The message to decode
- * @param scaled_imu2 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_scaled_imu2_decode(const mavlink_message_t* msg, mavlink_scaled_imu2_t* scaled_imu2)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	scaled_imu2->time_boot_ms = mavlink_msg_scaled_imu2_get_time_boot_ms(msg);
-	scaled_imu2->xacc = mavlink_msg_scaled_imu2_get_xacc(msg);
-	scaled_imu2->yacc = mavlink_msg_scaled_imu2_get_yacc(msg);
-	scaled_imu2->zacc = mavlink_msg_scaled_imu2_get_zacc(msg);
-	scaled_imu2->xgyro = mavlink_msg_scaled_imu2_get_xgyro(msg);
-	scaled_imu2->ygyro = mavlink_msg_scaled_imu2_get_ygyro(msg);
-	scaled_imu2->zgyro = mavlink_msg_scaled_imu2_get_zgyro(msg);
-	scaled_imu2->xmag = mavlink_msg_scaled_imu2_get_xmag(msg);
-	scaled_imu2->ymag = mavlink_msg_scaled_imu2_get_ymag(msg);
-	scaled_imu2->zmag = mavlink_msg_scaled_imu2_get_zmag(msg);
-#else
-	memcpy(scaled_imu2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU2_LEN);
-#endif
-}
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
deleted file mode 100644
index 10324bc94..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE SCALED_PRESSURE PACKING
-
-#define MAVLINK_MSG_ID_SCALED_PRESSURE 29
-
-typedef struct __mavlink_scaled_pressure_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float press_abs; ///< Absolute pressure (hectopascal)
- float press_diff; ///< Differential pressure 1 (hectopascal)
- int16_t temperature; ///< Temperature measurement (0.01 degrees celsius)
-} mavlink_scaled_pressure_t;
-
-#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 14
-#define MAVLINK_MSG_ID_29_LEN 14
-
-#define MAVLINK_MSG_ID_SCALED_PRESSURE_CRC 115
-#define MAVLINK_MSG_ID_29_CRC 115
-
-
-
-#define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \
-	"SCALED_PRESSURE", \
-	4, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure_t, time_boot_ms) }, \
-         { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure_t, press_abs) }, \
-         { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_diff) }, \
-         { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure_t, temperature) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a scaled_pressure 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 press_abs Absolute pressure (hectopascal)
- * @param press_diff Differential pressure 1 (hectopascal)
- * @param temperature Temperature measurement (0.01 degrees celsius)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
-#else
-	mavlink_scaled_pressure_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.press_abs = press_abs;
-	packet.press_diff = press_diff;
-	packet.temperature = temperature;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a scaled_pressure 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 press_abs Absolute pressure (hectopascal)
- * @param press_diff Differential pressure 1 (hectopascal)
- * @param temperature Temperature measurement (0.01 degrees celsius)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
-#else
-	mavlink_scaled_pressure_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.press_abs = press_abs;
-	packet.press_diff = press_diff;
-	packet.temperature = temperature;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a scaled_pressure 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 scaled_pressure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure)
-{
-	return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature);
-}
-
-/**
- * @brief Encode a scaled_pressure 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 scaled_pressure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure)
-{
-	return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature);
-}
-
-/**
- * @brief Send a scaled_pressure message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param press_abs Absolute pressure (hectopascal)
- * @param press_diff Differential pressure 1 (hectopascal)
- * @param temperature Temperature measurement (0.01 degrees celsius)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_scaled_pressure_send(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[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN];
-	_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;
-	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
-
-// MESSAGE SCALED_PRESSURE UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from scaled_pressure message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_scaled_pressure_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field press_abs from scaled_pressure message
- *
- * @return Absolute pressure (hectopascal)
- */
-static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field press_diff from scaled_pressure message
- *
- * @return Differential pressure 1 (hectopascal)
- */
-static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field temperature from scaled_pressure message
- *
- * @return Temperature measurement (0.01 degrees celsius)
- */
-static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Decode a scaled_pressure message into a struct
- *
- * @param msg The message to decode
- * @param scaled_pressure C-struct to decode the message contents into
- */
-static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* msg, mavlink_scaled_pressure_t* scaled_pressure)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	scaled_pressure->time_boot_ms = mavlink_msg_scaled_pressure_get_time_boot_ms(msg);
-	scaled_pressure->press_abs = mavlink_msg_scaled_pressure_get_press_abs(msg);
-	scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg);
-	scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg);
-#else
-	memcpy(scaled_pressure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_PRESSURE_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
deleted file mode 100644
index 6a14e93ed..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE SERVO_OUTPUT_RAW PACKING
-
-#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 36
-
-typedef struct __mavlink_servo_output_raw_t
-{
- uint32_t time_usec; ///< Timestamp (microseconds since system boot)
- uint16_t servo1_raw; ///< Servo output 1 value, in microseconds
- uint16_t servo2_raw; ///< Servo output 2 value, in microseconds
- uint16_t servo3_raw; ///< Servo output 3 value, in microseconds
- uint16_t servo4_raw; ///< Servo output 4 value, in microseconds
- uint16_t servo5_raw; ///< Servo output 5 value, in microseconds
- uint16_t servo6_raw; ///< Servo output 6 value, in microseconds
- uint16_t servo7_raw; ///< Servo output 7 value, in microseconds
- uint16_t servo8_raw; ///< Servo output 8 value, in microseconds
- uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
-} mavlink_servo_output_raw_t;
-
-#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 21
-#define MAVLINK_MSG_ID_36_LEN 21
-
-#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC 222
-#define MAVLINK_MSG_ID_36_CRC 222
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \
-	"SERVO_OUTPUT_RAW", \
-	10, \
-	{  { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \
-         { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \
-         { "servo2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_servo_output_raw_t, servo2_raw) }, \
-         { "servo3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_servo_output_raw_t, servo3_raw) }, \
-         { "servo4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_servo_output_raw_t, servo4_raw) }, \
-         { "servo5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_servo_output_raw_t, servo5_raw) }, \
-         { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \
-         { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \
-         { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \
-         { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a servo_output_raw 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_usec Timestamp (microseconds since system boot)
- * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
- * @param servo1_raw Servo output 1 value, in microseconds
- * @param servo2_raw Servo output 2 value, in microseconds
- * @param servo3_raw Servo output 3 value, in microseconds
- * @param servo4_raw Servo output 4 value, in microseconds
- * @param servo5_raw Servo output 5 value, in microseconds
- * @param servo6_raw Servo output 6 value, in microseconds
- * @param servo7_raw Servo output 7 value, in microseconds
- * @param servo8_raw Servo output 8 value, in microseconds
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
-#else
-	mavlink_servo_output_raw_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
-#endif
-}
-
-/**
- * @brief Pack a servo_output_raw 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_usec Timestamp (microseconds since system boot)
- * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
- * @param servo1_raw Servo output 1 value, in microseconds
- * @param servo2_raw Servo output 2 value, in microseconds
- * @param servo3_raw Servo output 3 value, in microseconds
- * @param servo4_raw Servo output 4 value, in microseconds
- * @param servo5_raw Servo output 5 value, in microseconds
- * @param servo6_raw Servo output 6 value, in microseconds
- * @param servo7_raw Servo output 7 value, in microseconds
- * @param servo8_raw Servo output 8 value, in microseconds
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
-#else
-	mavlink_servo_output_raw_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
-#endif
-}
-
-/**
- * @brief Encode a servo_output_raw 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 servo_output_raw C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw)
-{
-	return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw);
-}
-
-/**
- * @brief Encode a servo_output_raw 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 servo_output_raw C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw)
-{
-	return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw);
-}
-
-/**
- * @brief Send a servo_output_raw message
- * @param chan MAVLink channel to send the message
- *
- * @param time_usec Timestamp (microseconds since system boot)
- * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
- * @param servo1_raw Servo output 1 value, in microseconds
- * @param servo2_raw Servo output 2 value, in microseconds
- * @param servo3_raw Servo output 3 value, in microseconds
- * @param servo4_raw Servo output 4 value, in microseconds
- * @param servo5_raw Servo output 5 value, in microseconds
- * @param servo6_raw Servo output 6 value, in microseconds
- * @param servo7_raw Servo output 7 value, in microseconds
- * @param servo8_raw Servo output 8 value, in microseconds
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_servo_output_raw_send(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[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN];
-	_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;
-	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
-
-// MESSAGE SERVO_OUTPUT_RAW UNPACKING
-
-
-/**
- * @brief Get field time_usec from servo_output_raw message
- *
- * @return Timestamp (microseconds since system boot)
- */
-static inline uint32_t mavlink_msg_servo_output_raw_get_time_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field port from servo_output_raw message
- *
- * @return Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
- */
-static inline uint8_t mavlink_msg_servo_output_raw_get_port(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  20);
-}
-
-/**
- * @brief Get field servo1_raw from servo_output_raw message
- *
- * @return Servo output 1 value, in microseconds
- */
-static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field servo2_raw from servo_output_raw message
- *
- * @return Servo output 2 value, in microseconds
- */
-static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field servo3_raw from servo_output_raw message
- *
- * @return Servo output 3 value, in microseconds
- */
-static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field servo4_raw from servo_output_raw message
- *
- * @return Servo output 4 value, in microseconds
- */
-static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Get field servo5_raw from servo_output_raw message
- *
- * @return Servo output 5 value, in microseconds
- */
-static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field servo6_raw from servo_output_raw message
- *
- * @return Servo output 6 value, in microseconds
- */
-static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  14);
-}
-
-/**
- * @brief Get field servo7_raw from servo_output_raw message
- *
- * @return Servo output 7 value, in microseconds
- */
-static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field servo8_raw from servo_output_raw message
- *
- * @return Servo output 8 value, in microseconds
- */
-static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  18);
-}
-
-/**
- * @brief Decode a servo_output_raw message into a struct
- *
- * @param msg The message to decode
- * @param servo_output_raw C-struct to decode the message contents into
- */
-static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* msg, mavlink_servo_output_raw_t* servo_output_raw)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	servo_output_raw->time_usec = mavlink_msg_servo_output_raw_get_time_usec(msg);
-	servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg);
-	servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg);
-	servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg);
-	servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg);
-	servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg);
-	servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg);
-	servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg);
-	servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg);
-	servo_output_raw->port = mavlink_msg_servo_output_raw_get_port(msg);
-#else
-	memcpy(servo_output_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
-#endif
-}
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
deleted file mode 100644
index 6f0d7a69d..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT PACKING
-
-#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT 53
-
-typedef struct __mavlink_set_global_position_setpoint_int_t
-{
- int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7
- int32_t longitude; ///< Longitude (WGS84), in degrees * 1E7
- int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up)
- int16_t yaw; ///< Desired yaw angle in degrees * 100
- uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
-} mavlink_set_global_position_setpoint_int_t;
-
-#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN 15
-#define MAVLINK_MSG_ID_53_LEN 15
-
-#define MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC 33
-#define MAVLINK_MSG_ID_53_CRC 33
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT { \
-	"SET_GLOBAL_POSITION_SETPOINT_INT", \
-	5, \
-	{  { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_global_position_setpoint_int_t, latitude) }, \
-         { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_global_position_setpoint_int_t, longitude) }, \
-         { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_global_position_setpoint_int_t, altitude) }, \
-         { "yaw", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_set_global_position_setpoint_int_t, yaw) }, \
-         { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_set_global_position_setpoint_int_t, coordinate_frame) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_global_position_setpoint_int 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 coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @param yaw Desired yaw angle in degrees * 100
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#else
-	mavlink_set_global_position_setpoint_int_t packet;
-	packet.latitude = latitude;
-	packet.longitude = longitude;
-	packet.altitude = altitude;
-	packet.yaw = yaw;
-	packet.coordinate_frame = coordinate_frame;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_global_position_setpoint_int 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 coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @param yaw Desired yaw angle in degrees * 100
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#else
-	mavlink_set_global_position_setpoint_int_t packet;
-	packet.latitude = latitude;
-	packet.longitude = longitude;
-	packet.altitude = altitude;
-	packet.yaw = yaw;
-	packet.coordinate_frame = coordinate_frame;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_global_position_setpoint_int 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 set_global_position_setpoint_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int)
-{
-	return mavlink_msg_set_global_position_setpoint_int_pack(system_id, component_id, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw);
-}
-
-/**
- * @brief Encode a set_global_position_setpoint_int 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 set_global_position_setpoint_int C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int)
-{
-	return mavlink_msg_set_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw);
-}
-
-/**
- * @brief Send a set_global_position_setpoint_int message
- * @param chan MAVLink channel to send the message
- *
- * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84), in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @param yaw Desired yaw angle in degrees * 100
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_global_position_setpoint_int_send(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[MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN];
-	_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;
-	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
-
-// MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT UNPACKING
-
-
-/**
- * @brief Get field coordinate_frame from set_global_position_setpoint_int message
- *
- * @return Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT
- */
-static inline uint8_t mavlink_msg_set_global_position_setpoint_int_get_coordinate_frame(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Get field latitude from set_global_position_setpoint_int message
- *
- * @return Latitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field longitude from set_global_position_setpoint_int message
- *
- * @return Longitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field altitude from set_global_position_setpoint_int message
- *
- * @return Altitude (WGS84), in meters * 1000 (positive for up)
- */
-static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field yaw from set_global_position_setpoint_int message
- *
- * @return Desired yaw angle in degrees * 100
- */
-static inline int16_t mavlink_msg_set_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Decode a set_global_position_setpoint_int message into a struct
- *
- * @param msg The message to decode
- * @param set_global_position_setpoint_int C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_global_position_setpoint_int_decode(const mavlink_message_t* msg, mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_global_position_setpoint_int->latitude = mavlink_msg_set_global_position_setpoint_int_get_latitude(msg);
-	set_global_position_setpoint_int->longitude = mavlink_msg_set_global_position_setpoint_int_get_longitude(msg);
-	set_global_position_setpoint_int->altitude = mavlink_msg_set_global_position_setpoint_int_get_altitude(msg);
-	set_global_position_setpoint_int->yaw = mavlink_msg_set_global_position_setpoint_int_get_yaw(msg);
-	set_global_position_setpoint_int->coordinate_frame = mavlink_msg_set_global_position_setpoint_int_get_coordinate_frame(msg);
-#else
-	memcpy(set_global_position_setpoint_int, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
-#endif
-}
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
deleted file mode 100644
index c444d8d52..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE SET_GPS_GLOBAL_ORIGIN PACKING
-
-#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN 48
-
-typedef struct __mavlink_set_gps_global_origin_t
-{
- int32_t latitude; ///< Latitude (WGS84), in degrees * 1E7
- int32_t longitude; ///< Longitude (WGS84, in degrees * 1E7
- int32_t altitude; ///< Altitude (WGS84), in meters * 1000 (positive for up)
- uint8_t target_system; ///< System ID
-} mavlink_set_gps_global_origin_t;
-
-#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN 13
-#define MAVLINK_MSG_ID_48_LEN 13
-
-#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC 41
-#define MAVLINK_MSG_ID_48_CRC 41
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN { \
-	"SET_GPS_GLOBAL_ORIGIN", \
-	4, \
-	{  { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_gps_global_origin_t, latitude) }, \
-         { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_gps_global_origin_t, longitude) }, \
-         { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_gps_global_origin_t, altitude) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_set_gps_global_origin_t, target_system) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_gps_global_origin 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 target_system System ID
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84, in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
-#else
-	mavlink_set_gps_global_origin_t packet;
-	packet.latitude = latitude;
-	packet.longitude = longitude;
-	packet.altitude = altitude;
-	packet.target_system = target_system;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_gps_global_origin 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 target_system System ID
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84, in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
-#else
-	mavlink_set_gps_global_origin_t packet;
-	packet.latitude = latitude;
-	packet.longitude = longitude;
-	packet.altitude = altitude;
-	packet.target_system = target_system;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_gps_global_origin 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 set_gps_global_origin C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin)
-{
-	return mavlink_msg_set_gps_global_origin_pack(system_id, component_id, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude);
-}
-
-/**
- * @brief Encode a set_gps_global_origin 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 set_gps_global_origin C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin)
-{
-	return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude);
-}
-
-/**
- * @brief Send a set_gps_global_origin message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param latitude Latitude (WGS84), in degrees * 1E7
- * @param longitude Longitude (WGS84, in degrees * 1E7
- * @param altitude Altitude (WGS84), in meters * 1000 (positive for up)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_gps_global_origin_send(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[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN];
-	_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;
-	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
-
-// MESSAGE SET_GPS_GLOBAL_ORIGIN UNPACKING
-
-
-/**
- * @brief Get field target_system from set_gps_global_origin message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_set_gps_global_origin_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field latitude from set_gps_global_origin message
- *
- * @return Latitude (WGS84), in degrees * 1E7
- */
-static inline int32_t mavlink_msg_set_gps_global_origin_get_latitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field longitude from set_gps_global_origin message
- *
- * @return Longitude (WGS84, in degrees * 1E7
- */
-static inline int32_t mavlink_msg_set_gps_global_origin_get_longitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field altitude from set_gps_global_origin message
- *
- * @return Altitude (WGS84), in meters * 1000 (positive for up)
- */
-static inline int32_t mavlink_msg_set_gps_global_origin_get_altitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Decode a set_gps_global_origin message into a struct
- *
- * @param msg The message to decode
- * @param set_gps_global_origin C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_gps_global_origin_decode(const mavlink_message_t* msg, mavlink_set_gps_global_origin_t* set_gps_global_origin)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_gps_global_origin->latitude = mavlink_msg_set_gps_global_origin_get_latitude(msg);
-	set_gps_global_origin->longitude = mavlink_msg_set_gps_global_origin_get_longitude(msg);
-	set_gps_global_origin->altitude = mavlink_msg_set_gps_global_origin_get_altitude(msg);
-	set_gps_global_origin->target_system = mavlink_msg_set_gps_global_origin_get_target_system(msg);
-#else
-	memcpy(set_gps_global_origin, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
-#endif
-}
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
deleted file mode 100644
index 6f2835e03..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE SET_LOCAL_POSITION_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT 50
-
-typedef struct __mavlink_set_local_position_setpoint_t
-{
- float x; ///< x position
- float y; ///< y position
- float z; ///< z position
- float yaw; ///< Desired yaw angle
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t coordinate_frame; ///< Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
-} mavlink_set_local_position_setpoint_t;
-
-#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN 19
-#define MAVLINK_MSG_ID_50_LEN 19
-
-#define MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC 214
-#define MAVLINK_MSG_ID_50_CRC 214
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT { \
-	"SET_LOCAL_POSITION_SETPOINT", \
-	7, \
-	{  { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_local_position_setpoint_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_local_position_setpoint_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_local_position_setpoint_t, z) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_local_position_setpoint_t, yaw) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_local_position_setpoint_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_local_position_setpoint_t, target_component) }, \
-         { "coordinate_frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_set_local_position_setpoint_t, coordinate_frame) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_local_position_setpoint 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 target_system System ID
- * @param target_component Component ID
- * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw Desired yaw angle
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
-#else
-	mavlink_set_local_position_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_local_position_setpoint 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 target_system System ID
- * @param target_component Component ID
- * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw Desired yaw angle
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
-#else
-	mavlink_set_local_position_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_local_position_setpoint 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 set_local_position_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint)
-{
-	return mavlink_msg_set_local_position_setpoint_pack(system_id, component_id, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw);
-}
-
-/**
- * @brief Encode a set_local_position_setpoint 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 set_local_position_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint)
-{
-	return mavlink_msg_set_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw);
-}
-
-/**
- * @brief Send a set_local_position_setpoint message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw Desired yaw angle
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_local_position_setpoint_send(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[MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN];
-	_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;
-	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
-
-// MESSAGE SET_LOCAL_POSITION_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field target_system from set_local_position_setpoint message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field target_component from set_local_position_setpoint message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  17);
-}
-
-/**
- * @brief Get field coordinate_frame from set_local_position_setpoint message
- *
- * @return Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU
- */
-static inline uint8_t mavlink_msg_set_local_position_setpoint_get_coordinate_frame(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  18);
-}
-
-/**
- * @brief Get field x from set_local_position_setpoint message
- *
- * @return x position
- */
-static inline float mavlink_msg_set_local_position_setpoint_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field y from set_local_position_setpoint message
- *
- * @return y position
- */
-static inline float mavlink_msg_set_local_position_setpoint_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field z from set_local_position_setpoint message
- *
- * @return z position
- */
-static inline float mavlink_msg_set_local_position_setpoint_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw from set_local_position_setpoint message
- *
- * @return Desired yaw angle
- */
-static inline float mavlink_msg_set_local_position_setpoint_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Decode a set_local_position_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param set_local_position_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_set_local_position_setpoint_t* set_local_position_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_local_position_setpoint->x = mavlink_msg_set_local_position_setpoint_get_x(msg);
-	set_local_position_setpoint->y = mavlink_msg_set_local_position_setpoint_get_y(msg);
-	set_local_position_setpoint->z = mavlink_msg_set_local_position_setpoint_get_z(msg);
-	set_local_position_setpoint->yaw = mavlink_msg_set_local_position_setpoint_get_yaw(msg);
-	set_local_position_setpoint->target_system = mavlink_msg_set_local_position_setpoint_get_target_system(msg);
-	set_local_position_setpoint->target_component = mavlink_msg_set_local_position_setpoint_get_target_component(msg);
-	set_local_position_setpoint->coordinate_frame = mavlink_msg_set_local_position_setpoint_get_coordinate_frame(msg);
-#else
-	memcpy(set_local_position_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
-#endif
-}
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
deleted file mode 100644
index 1aff42cce..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE SET_MODE PACKING
-
-#define MAVLINK_MSG_ID_SET_MODE 11
-
-typedef struct __mavlink_set_mode_t
-{
- uint32_t custom_mode; ///< The new autopilot-specific mode. This field can be ignored by an autopilot.
- uint8_t target_system; ///< The system setting the mode
- uint8_t base_mode; ///< The new base mode
-} mavlink_set_mode_t;
-
-#define MAVLINK_MSG_ID_SET_MODE_LEN 6
-#define MAVLINK_MSG_ID_11_LEN 6
-
-#define MAVLINK_MSG_ID_SET_MODE_CRC 89
-#define MAVLINK_MSG_ID_11_CRC 89
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_MODE { \
-	"SET_MODE", \
-	3, \
-	{  { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_mode_t, custom_mode) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_set_mode_t, target_system) }, \
-         { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_set_mode_t, base_mode) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_mode 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 target_system The system setting the mode
- * @param base_mode The new base mode
- * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t base_mode, uint32_t custom_mode)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SET_MODE_LEN];
-	_mav_put_uint32_t(buf, 0, custom_mode);
-	_mav_put_uint8_t(buf, 4, target_system);
-	_mav_put_uint8_t(buf, 5, base_mode);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MODE_LEN);
-#else
-	mavlink_set_mode_t packet;
-	packet.custom_mode = custom_mode;
-	packet.target_system = target_system;
-	packet.base_mode = base_mode;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_MODE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_mode 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 target_system The system setting the mode
- * @param base_mode The new base mode
- * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t base_mode,uint32_t custom_mode)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SET_MODE_LEN];
-	_mav_put_uint32_t(buf, 0, custom_mode);
-	_mav_put_uint8_t(buf, 4, target_system);
-	_mav_put_uint8_t(buf, 5, base_mode);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MODE_LEN);
-#else
-	mavlink_set_mode_t packet;
-	packet.custom_mode = custom_mode;
-	packet.target_system = target_system;
-	packet.base_mode = base_mode;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_MODE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_mode 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 set_mode C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode)
-{
-	return mavlink_msg_set_mode_pack(system_id, component_id, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode);
-}
-
-/**
- * @brief Encode a set_mode 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 set_mode C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_mode_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode)
-{
-	return mavlink_msg_set_mode_pack_chan(system_id, component_id, chan, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode);
-}
-
-/**
- * @brief Send a set_mode message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system The system setting the mode
- * @param base_mode The new base mode
- * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_mode_send(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[MAVLINK_MSG_ID_SET_MODE_LEN];
-	_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;
-	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
-
-// MESSAGE SET_MODE UNPACKING
-
-
-/**
- * @brief Get field target_system from set_mode message
- *
- * @return The system setting the mode
- */
-static inline uint8_t mavlink_msg_set_mode_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field base_mode from set_mode message
- *
- * @return The new base mode
- */
-static inline uint8_t mavlink_msg_set_mode_get_base_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field custom_mode from set_mode message
- *
- * @return The new autopilot-specific mode. This field can be ignored by an autopilot.
- */
-static inline uint32_t mavlink_msg_set_mode_get_custom_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Decode a set_mode message into a struct
- *
- * @param msg The message to decode
- * @param set_mode C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_mode_decode(const mavlink_message_t* msg, mavlink_set_mode_t* set_mode)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_mode->custom_mode = mavlink_msg_set_mode_get_custom_mode(msg);
-	set_mode->target_system = mavlink_msg_set_mode_get_target_system(msg);
-	set_mode->base_mode = mavlink_msg_set_mode_get_base_mode(msg);
-#else
-	memcpy(set_mode, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_MODE_LEN);
-#endif
-}
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
deleted file mode 100644
index 8ceb8888f..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE SET_QUAD_MOTORS_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT 60
-
-typedef struct __mavlink_set_quad_motors_setpoint_t
-{
- uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration
- uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration
- uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration
- uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration
- uint8_t target_system; ///< System ID of the system that should set these motor commands
-} mavlink_set_quad_motors_setpoint_t;
-
-#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN 9
-#define MAVLINK_MSG_ID_60_LEN 9
-
-#define MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC 30
-#define MAVLINK_MSG_ID_60_CRC 30
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT { \
-	"SET_QUAD_MOTORS_SETPOINT", \
-	5, \
-	{  { "motor_front_nw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_set_quad_motors_setpoint_t, motor_front_nw) }, \
-         { "motor_right_ne", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_set_quad_motors_setpoint_t, motor_right_ne) }, \
-         { "motor_back_se", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_set_quad_motors_setpoint_t, motor_back_se) }, \
-         { "motor_left_sw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_set_quad_motors_setpoint_t, motor_left_sw) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_set_quad_motors_setpoint_t, target_system) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_quad_motors_setpoint 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 target_system System ID of the system that should set these motor commands
- * @param motor_front_nw Front motor in + configuration, front left motor in x configuration
- * @param motor_right_ne Right motor in + configuration, front right motor in x configuration
- * @param motor_back_se Back motor in + configuration, back right motor in x configuration
- * @param motor_left_sw Left motor in + configuration, back left motor in x configuration
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
-#else
-	mavlink_set_quad_motors_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_quad_motors_setpoint 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 target_system System ID of the system that should set these motor commands
- * @param motor_front_nw Front motor in + configuration, front left motor in x configuration
- * @param motor_right_ne Right motor in + configuration, front right motor in x configuration
- * @param motor_back_se Back motor in + configuration, back right motor in x configuration
- * @param motor_left_sw Left motor in + configuration, back left motor in x configuration
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
-#else
-	mavlink_set_quad_motors_setpoint_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_quad_motors_setpoint 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 set_quad_motors_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint)
-{
-	return mavlink_msg_set_quad_motors_setpoint_pack(system_id, component_id, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw);
-}
-
-/**
- * @brief Encode a set_quad_motors_setpoint 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 set_quad_motors_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint)
-{
-	return mavlink_msg_set_quad_motors_setpoint_pack_chan(system_id, component_id, chan, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw);
-}
-
-/**
- * @brief Send a set_quad_motors_setpoint message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID of the system that should set these motor commands
- * @param motor_front_nw Front motor in + configuration, front left motor in x configuration
- * @param motor_right_ne Right motor in + configuration, front right motor in x configuration
- * @param motor_back_se Back motor in + configuration, back right motor in x configuration
- * @param motor_left_sw Left motor in + configuration, back left motor in x configuration
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_quad_motors_setpoint_send(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[MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN];
-	_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;
-	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
-
-// MESSAGE SET_QUAD_MOTORS_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field target_system from set_quad_motors_setpoint message
- *
- * @return System ID of the system that should set these motor commands
- */
-static inline uint8_t mavlink_msg_set_quad_motors_setpoint_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field motor_front_nw from set_quad_motors_setpoint message
- *
- * @return Front motor in + configuration, front left motor in x configuration
- */
-static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_front_nw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field motor_right_ne from set_quad_motors_setpoint message
- *
- * @return Right motor in + configuration, front right motor in x configuration
- */
-static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_right_ne(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Get field motor_back_se from set_quad_motors_setpoint message
- *
- * @return Back motor in + configuration, back right motor in x configuration
- */
-static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_back_se(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field motor_left_sw from set_quad_motors_setpoint message
- *
- * @return Left motor in + configuration, back left motor in x configuration
- */
-static inline uint16_t mavlink_msg_set_quad_motors_setpoint_get_motor_left_sw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Decode a set_quad_motors_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param set_quad_motors_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_quad_motors_setpoint_decode(const mavlink_message_t* msg, mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_quad_motors_setpoint->motor_front_nw = mavlink_msg_set_quad_motors_setpoint_get_motor_front_nw(msg);
-	set_quad_motors_setpoint->motor_right_ne = mavlink_msg_set_quad_motors_setpoint_get_motor_right_ne(msg);
-	set_quad_motors_setpoint->motor_back_se = mavlink_msg_set_quad_motors_setpoint_get_motor_back_se(msg);
-	set_quad_motors_setpoint->motor_left_sw = mavlink_msg_set_quad_motors_setpoint_get_motor_left_sw(msg);
-	set_quad_motors_setpoint->target_system = mavlink_msg_set_quad_motors_setpoint_get_target_system(msg);
-#else
-	memcpy(set_quad_motors_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
-#endif
-}
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
deleted file mode 100644
index 9ef294cc9..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h
+++ /dev/null
@@ -1,353 +0,0 @@
-// MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST PACKING
-
-#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST 63
-
-typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t
-{
- int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX)
- int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX)
- int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX)
- uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported)
- uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported)
- uint8_t led_red[4]; ///< RGB red channel (0-255)
- uint8_t led_blue[4]; ///< RGB green channel (0-255)
- uint8_t led_green[4]; ///< RGB blue channel (0-255)
-} mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t;
-
-#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN 46
-#define MAVLINK_MSG_ID_63_LEN 46
-
-#define MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC 130
-#define MAVLINK_MSG_ID_63_CRC 130
-
-#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_ROLL_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_PITCH_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_YAW_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_THRUST_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_RED_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_BLUE_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_FIELD_LED_GREEN_LEN 4
-
-#define MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST { \
-	"SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST", \
-	9, \
-	{  { "roll", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_INT16_T, 4, 8, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_INT16_T, 4, 16, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, yaw) }, \
-         { "thrust", NULL, MAVLINK_TYPE_UINT16_T, 4, 24, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, thrust) }, \
-         { "group", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, group) }, \
-         { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, mode) }, \
-         { "led_red", NULL, MAVLINK_TYPE_UINT8_T, 4, 34, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_red) }, \
-         { "led_blue", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_blue) }, \
-         { "led_green", NULL, MAVLINK_TYPE_UINT8_T, 4, 42, offsetof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t, led_green) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust 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 group ID of the quadrotor group (0 - 255, up to 256 groups supported)
- * @param mode ID of the flight mode (0 - 255, up to 256 modes supported)
- * @param led_red RGB red channel (0-255)
- * @param led_blue RGB green channel (0-255)
- * @param led_green RGB blue channel (0-255)
- * @param roll Desired roll angle in radians +-PI (+-INT16_MAX)
- * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
-#else
-	mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, 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
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust 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 group ID of the quadrotor group (0 - 255, up to 256 groups supported)
- * @param mode ID of the flight mode (0 - 255, up to 256 modes supported)
- * @param led_red RGB red channel (0-255)
- * @param led_blue RGB green channel (0-255)
- * @param led_green RGB blue channel (0-255)
- * @param roll Desired roll angle in radians +-PI (+-INT16_MAX)
- * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
-#else
-	mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 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
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust 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 set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust)
-{
-	return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust);
-}
-
-/**
- * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust 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 set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust)
-{
-	return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust);
-}
-
-/**
- * @brief Send a set_quad_swarm_led_roll_pitch_yaw_thrust message
- * @param chan MAVLink channel to send the message
- *
- * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported)
- * @param mode ID of the flight mode (0 - 255, up to 256 modes supported)
- * @param led_red RGB red channel (0-255)
- * @param led_blue RGB green channel (0-255)
- * @param led_green RGB blue channel (0-255)
- * @param roll Desired roll angle in radians +-PI (+-INT16_MAX)
- * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(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[MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN];
-	_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;
-	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
-
-// MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST UNPACKING
-
-
-/**
- * @brief Get field group from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return ID of the quadrotor group (0 - 255, up to 256 groups supported)
- */
-static inline uint8_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_group(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  32);
-}
-
-/**
- * @brief Get field mode from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return ID of the flight mode (0 - 255, up to 256 modes supported)
- */
-static inline uint8_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  33);
-}
-
-/**
- * @brief Get field led_red from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return RGB red channel (0-255)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_red(const mavlink_message_t* msg, uint8_t *led_red)
-{
-	return _MAV_RETURN_uint8_t_array(msg, led_red, 4,  34);
-}
-
-/**
- * @brief Get field led_blue from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return RGB green channel (0-255)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_blue(const mavlink_message_t* msg, uint8_t *led_blue)
-{
-	return _MAV_RETURN_uint8_t_array(msg, led_blue, 4,  38);
-}
-
-/**
- * @brief Get field led_green from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return RGB blue channel (0-255)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_green(const mavlink_message_t* msg, uint8_t *led_green)
-{
-	return _MAV_RETURN_uint8_t_array(msg, led_green, 4,  42);
-}
-
-/**
- * @brief Get field roll from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return Desired roll angle in radians +-PI (+-INT16_MAX)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll)
-{
-	return _MAV_RETURN_int16_t_array(msg, roll, 4,  0);
-}
-
-/**
- * @brief Get field pitch from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return Desired pitch angle in radians +-PI (+-INT16_MAX)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch)
-{
-	return _MAV_RETURN_int16_t_array(msg, pitch, 4,  8);
-}
-
-/**
- * @brief Get field yaw from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw)
-{
-	return _MAV_RETURN_int16_t_array(msg, yaw, 4,  16);
-}
-
-/**
- * @brief Get field thrust from set_quad_swarm_led_roll_pitch_yaw_thrust message
- *
- * @return Collective thrust, scaled to uint16 (0..UINT16_MAX)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust)
-{
-	return _MAV_RETURN_uint16_t_array(msg, thrust, 4,  24);
-}
-
-/**
- * @brief Decode a set_quad_swarm_led_roll_pitch_yaw_thrust message into a struct
- *
- * @param msg The message to decode
- * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->roll);
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch);
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw);
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust);
-	set_quad_swarm_led_roll_pitch_yaw_thrust->group = mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_group(msg);
-	set_quad_swarm_led_roll_pitch_yaw_thrust->mode = mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_mode(msg);
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_red(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red);
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_blue(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue);
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_led_green(msg, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green);
-#else
-	memcpy(set_quad_swarm_led_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
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
deleted file mode 100644
index 7d8d526f8..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h
+++ /dev/null
@@ -1,284 +0,0 @@
-// MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST PACKING
-
-#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST 61
-
-typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t
-{
- int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX)
- int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX)
- int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX)
- uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported)
- uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported)
-} mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t;
-
-#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN 34
-#define MAVLINK_MSG_ID_61_LEN 34
-
-#define MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC 240
-#define MAVLINK_MSG_ID_61_CRC 240
-
-#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_ROLL_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_PITCH_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_YAW_LEN 4
-#define MAVLINK_MSG_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_FIELD_THRUST_LEN 4
-
-#define MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST { \
-	"SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST", \
-	6, \
-	{  { "roll", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_INT16_T, 4, 8, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_INT16_T, 4, 16, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, yaw) }, \
-         { "thrust", NULL, MAVLINK_TYPE_UINT16_T, 4, 24, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, thrust) }, \
-         { "group", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, group) }, \
-         { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t, mode) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust 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 group ID of the quadrotor group (0 - 255, up to 256 groups supported)
- * @param mode ID of the flight mode (0 - 255, up to 256 modes supported)
- * @param roll Desired roll angle in radians +-PI (+-INT16_MAX)
- * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
-#else
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust 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 group ID of the quadrotor group (0 - 255, up to 256 groups supported)
- * @param mode ID of the flight mode (0 - 255, up to 256 modes supported)
- * @param roll Desired roll angle in radians +-PI (+-INT16_MAX)
- * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
-#else
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust 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 set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust)
-{
-	return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust);
-}
-
-/**
- * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust 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 set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust)
-{
-	return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust);
-}
-
-/**
- * @brief Send a set_quad_swarm_roll_pitch_yaw_thrust message
- * @param chan MAVLink channel to send the message
- *
- * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported)
- * @param mode ID of the flight mode (0 - 255, up to 256 modes supported)
- * @param roll Desired roll angle in radians +-PI (+-INT16_MAX)
- * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX)
- * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send(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[MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN];
-	_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;
-	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
-
-// MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST UNPACKING
-
-
-/**
- * @brief Get field group from set_quad_swarm_roll_pitch_yaw_thrust message
- *
- * @return ID of the quadrotor group (0 - 255, up to 256 groups supported)
- */
-static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_group(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  32);
-}
-
-/**
- * @brief Get field mode from set_quad_swarm_roll_pitch_yaw_thrust message
- *
- * @return ID of the flight mode (0 - 255, up to 256 modes supported)
- */
-static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  33);
-}
-
-/**
- * @brief Get field roll from set_quad_swarm_roll_pitch_yaw_thrust message
- *
- * @return Desired roll angle in radians +-PI (+-INT16_MAX)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll)
-{
-	return _MAV_RETURN_int16_t_array(msg, roll, 4,  0);
-}
-
-/**
- * @brief Get field pitch from set_quad_swarm_roll_pitch_yaw_thrust message
- *
- * @return Desired pitch angle in radians +-PI (+-INT16_MAX)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch)
-{
-	return _MAV_RETURN_int16_t_array(msg, pitch, 4,  8);
-}
-
-/**
- * @brief Get field yaw from set_quad_swarm_roll_pitch_yaw_thrust message
- *
- * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw)
-{
-	return _MAV_RETURN_int16_t_array(msg, yaw, 4,  16);
-}
-
-/**
- * @brief Get field thrust from set_quad_swarm_roll_pitch_yaw_thrust message
- *
- * @return Collective thrust, scaled to uint16 (0..UINT16_MAX)
- */
-static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust)
-{
-	return _MAV_RETURN_uint16_t_array(msg, thrust, 4,  24);
-}
-
-/**
- * @brief Decode a set_quad_swarm_roll_pitch_yaw_thrust message into a struct
- *
- * @param msg The message to decode
- * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(msg, set_quad_swarm_roll_pitch_yaw_thrust->roll);
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(msg, set_quad_swarm_roll_pitch_yaw_thrust->pitch);
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(msg, set_quad_swarm_roll_pitch_yaw_thrust->yaw);
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(msg, set_quad_swarm_roll_pitch_yaw_thrust->thrust);
-	set_quad_swarm_roll_pitch_yaw_thrust->group = mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_group(msg);
-	set_quad_swarm_roll_pitch_yaw_thrust->mode = mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode(msg);
-#else
-	memcpy(set_quad_swarm_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
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
deleted file mode 100644
index 5846ba41f..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST PACKING
-
-#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 57
-
-typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t
-{
- float roll_speed; ///< Desired roll angular speed in rad/s
- float pitch_speed; ///< Desired pitch angular speed in rad/s
- float yaw_speed; ///< Desired yaw angular speed in rad/s
- float thrust; ///< Collective thrust, normalized to 0 .. 1
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_set_roll_pitch_yaw_speed_thrust_t;
-
-#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN 18
-#define MAVLINK_MSG_ID_57_LEN 18
-
-#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC 24
-#define MAVLINK_MSG_ID_57_CRC 24
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST { \
-	"SET_ROLL_PITCH_YAW_SPEED_THRUST", \
-	6, \
-	{  { "roll_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, roll_speed) }, \
-         { "pitch_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, pitch_speed) }, \
-         { "yaw_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, yaw_speed) }, \
-         { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, thrust) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_roll_pitch_yaw_speed_thrust 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 target_system System ID
- * @param target_component Component ID
- * @param roll_speed Desired roll angular speed in rad/s
- * @param pitch_speed Desired pitch angular speed in rad/s
- * @param yaw_speed Desired yaw angular speed in rad/s
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
-#else
-	mavlink_set_roll_pitch_yaw_speed_thrust_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_roll_pitch_yaw_speed_thrust 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 target_system System ID
- * @param target_component Component ID
- * @param roll_speed Desired roll angular speed in rad/s
- * @param pitch_speed Desired pitch angular speed in rad/s
- * @param yaw_speed Desired yaw angular speed in rad/s
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
-#else
-	mavlink_set_roll_pitch_yaw_speed_thrust_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_roll_pitch_yaw_speed_thrust 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 set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust)
-{
-	return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust);
-}
-
-/**
- * @brief Encode a set_roll_pitch_yaw_speed_thrust 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 set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust)
-{
-	return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust);
-}
-
-/**
- * @brief Send a set_roll_pitch_yaw_speed_thrust message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param roll_speed Desired roll angular speed in rad/s
- * @param pitch_speed Desired pitch angular speed in rad/s
- * @param yaw_speed Desired yaw angular speed in rad/s
- * @param thrust Collective thrust, normalized to 0 .. 1
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(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[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN];
-	_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;
-	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
-
-// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING
-
-
-/**
- * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field target_component from set_roll_pitch_yaw_speed_thrust message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  17);
-}
-
-/**
- * @brief Get field roll_speed from set_roll_pitch_yaw_speed_thrust message
- *
- * @return Desired roll angular speed in rad/s
- */
-static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field pitch_speed from set_roll_pitch_yaw_speed_thrust message
- *
- * @return Desired pitch angular speed in rad/s
- */
-static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field yaw_speed from set_roll_pitch_yaw_speed_thrust message
- *
- * @return Desired yaw angular speed in rad/s
- */
-static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field thrust from set_roll_pitch_yaw_speed_thrust message
- *
- * @return Collective thrust, normalized to 0 .. 1
- */
-static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Decode a set_roll_pitch_yaw_speed_thrust message into a struct
- *
- * @param msg The message to decode
- * @param set_roll_pitch_yaw_speed_thrust C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_roll_pitch_yaw_speed_thrust->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(msg);
-	set_roll_pitch_yaw_speed_thrust->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(msg);
-	set_roll_pitch_yaw_speed_thrust->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(msg);
-	set_roll_pitch_yaw_speed_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(msg);
-	set_roll_pitch_yaw_speed_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(msg);
-	set_roll_pitch_yaw_speed_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(msg);
-#else
-	memcpy(set_roll_pitch_yaw_speed_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
-#endif
-}
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
deleted file mode 100644
index 334fd39e3..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE SET_ROLL_PITCH_YAW_THRUST PACKING
-
-#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 56
-
-typedef struct __mavlink_set_roll_pitch_yaw_thrust_t
-{
- float roll; ///< Desired roll angle in radians
- float pitch; ///< Desired pitch angle in radians
- float yaw; ///< Desired yaw angle in radians
- float thrust; ///< Collective thrust, normalized to 0 .. 1
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_set_roll_pitch_yaw_thrust_t;
-
-#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN 18
-#define MAVLINK_MSG_ID_56_LEN 18
-
-#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC 100
-#define MAVLINK_MSG_ID_56_CRC 100
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST { \
-	"SET_ROLL_PITCH_YAW_THRUST", \
-	6, \
-	{  { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, yaw) }, \
-         { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, thrust) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_roll_pitch_yaw_thrust 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 target_system System ID
- * @param target_component Component ID
- * @param roll Desired roll angle in radians
- * @param pitch Desired pitch angle in radians
- * @param yaw Desired yaw angle in radians
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
-#else
-	mavlink_set_roll_pitch_yaw_thrust_t packet;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.thrust = thrust;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_roll_pitch_yaw_thrust 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 target_system System ID
- * @param target_component Component ID
- * @param roll Desired roll angle in radians
- * @param pitch Desired pitch angle in radians
- * @param yaw Desired yaw angle in radians
- * @param thrust Collective thrust, normalized to 0 .. 1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
-#else
-	mavlink_set_roll_pitch_yaw_thrust_t packet;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.thrust = thrust;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_roll_pitch_yaw_thrust 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 set_roll_pitch_yaw_thrust C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust)
-{
-	return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust);
-}
-
-/**
- * @brief Encode a set_roll_pitch_yaw_thrust 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 set_roll_pitch_yaw_thrust C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust)
-{
-	return mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust);
-}
-
-/**
- * @brief Send a set_roll_pitch_yaw_thrust message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param roll Desired roll angle in radians
- * @param pitch Desired pitch angle in radians
- * @param yaw Desired yaw angle in radians
- * @param thrust Collective thrust, normalized to 0 .. 1
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(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[MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN];
-	_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;
-	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
-
-// MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING
-
-
-/**
- * @brief Get field target_system from set_roll_pitch_yaw_thrust message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field target_component from set_roll_pitch_yaw_thrust message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  17);
-}
-
-/**
- * @brief Get field roll from set_roll_pitch_yaw_thrust message
- *
- * @return Desired roll angle in radians
- */
-static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field pitch from set_roll_pitch_yaw_thrust message
- *
- * @return Desired pitch angle in radians
- */
-static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field yaw from set_roll_pitch_yaw_thrust message
- *
- * @return Desired yaw angle in radians
- */
-static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field thrust from set_roll_pitch_yaw_thrust message
- *
- * @return Collective thrust, normalized to 0 .. 1
- */
-static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Decode a set_roll_pitch_yaw_thrust message into a struct
- *
- * @param msg The message to decode
- * @param set_roll_pitch_yaw_thrust C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_roll_pitch_yaw_thrust->roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(msg);
-	set_roll_pitch_yaw_thrust->pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(msg);
-	set_roll_pitch_yaw_thrust->yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(msg);
-	set_roll_pitch_yaw_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(msg);
-	set_roll_pitch_yaw_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(msg);
-	set_roll_pitch_yaw_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(msg);
-#else
-	memcpy(set_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
-#endif
-}
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
deleted file mode 100644
index 93ce345b6..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE SETPOINT_6DOF PACKING
-
-#define MAVLINK_MSG_ID_SETPOINT_6DOF 149
-
-typedef struct __mavlink_setpoint_6dof_t
-{
- float trans_x; ///< Translational Component in x
- float trans_y; ///< Translational Component in y
- float trans_z; ///< Translational Component in z
- float rot_x; ///< Rotational Component in x
- float rot_y; ///< Rotational Component in y
- float rot_z; ///< Rotational Component in z
- uint8_t target_system; ///< System ID
-} mavlink_setpoint_6dof_t;
-
-#define MAVLINK_MSG_ID_SETPOINT_6DOF_LEN 25
-#define MAVLINK_MSG_ID_149_LEN 25
-
-#define MAVLINK_MSG_ID_SETPOINT_6DOF_CRC 15
-#define MAVLINK_MSG_ID_149_CRC 15
-
-
-
-#define MAVLINK_MESSAGE_INFO_SETPOINT_6DOF { \
-	"SETPOINT_6DOF", \
-	7, \
-	{  { "trans_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_setpoint_6dof_t, trans_x) }, \
-         { "trans_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_setpoint_6dof_t, trans_y) }, \
-         { "trans_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_setpoint_6dof_t, trans_z) }, \
-         { "rot_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_setpoint_6dof_t, rot_x) }, \
-         { "rot_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_setpoint_6dof_t, rot_y) }, \
-         { "rot_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_setpoint_6dof_t, rot_z) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_setpoint_6dof_t, target_system) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a setpoint_6dof 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 target_system System ID
- * @param trans_x Translational Component in x
- * @param trans_y Translational Component in y
- * @param trans_z Translational Component in z
- * @param rot_x Rotational Component in x
- * @param rot_y Rotational Component in y
- * @param rot_z Rotational Component in z
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
-#else
-	mavlink_setpoint_6dof_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
-#endif
-}
-
-/**
- * @brief Pack a setpoint_6dof 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 target_system System ID
- * @param trans_x Translational Component in x
- * @param trans_y Translational Component in y
- * @param trans_z Translational Component in z
- * @param rot_x Rotational Component in x
- * @param rot_y Rotational Component in y
- * @param rot_z Rotational Component in z
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
-#else
-	mavlink_setpoint_6dof_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SETPOINT_6DOF;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
-#endif
-}
-
-/**
- * @brief Encode a setpoint_6dof 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 setpoint_6dof C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof)
-{
-	return mavlink_msg_setpoint_6dof_pack(system_id, component_id, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z);
-}
-
-/**
- * @brief Encode a setpoint_6dof 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 setpoint_6dof C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_setpoint_6dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof)
-{
-	return mavlink_msg_setpoint_6dof_pack_chan(system_id, component_id, chan, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z);
-}
-
-/**
- * @brief Send a setpoint_6dof message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param trans_x Translational Component in x
- * @param trans_y Translational Component in y
- * @param trans_z Translational Component in z
- * @param rot_x Rotational Component in x
- * @param rot_y Rotational Component in y
- * @param rot_z Rotational Component in z
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_setpoint_6dof_send(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[MAVLINK_MSG_ID_SETPOINT_6DOF_LEN];
-	_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;
-	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
-
-// MESSAGE SETPOINT_6DOF UNPACKING
-
-
-/**
- * @brief Get field target_system from setpoint_6dof message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_setpoint_6dof_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  24);
-}
-
-/**
- * @brief Get field trans_x from setpoint_6dof message
- *
- * @return Translational Component in x
- */
-static inline float mavlink_msg_setpoint_6dof_get_trans_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field trans_y from setpoint_6dof message
- *
- * @return Translational Component in y
- */
-static inline float mavlink_msg_setpoint_6dof_get_trans_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field trans_z from setpoint_6dof message
- *
- * @return Translational Component in z
- */
-static inline float mavlink_msg_setpoint_6dof_get_trans_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field rot_x from setpoint_6dof message
- *
- * @return Rotational Component in x
- */
-static inline float mavlink_msg_setpoint_6dof_get_rot_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field rot_y from setpoint_6dof message
- *
- * @return Rotational Component in y
- */
-static inline float mavlink_msg_setpoint_6dof_get_rot_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field rot_z from setpoint_6dof message
- *
- * @return Rotational Component in z
- */
-static inline float mavlink_msg_setpoint_6dof_get_rot_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Decode a setpoint_6dof message into a struct
- *
- * @param msg The message to decode
- * @param setpoint_6dof C-struct to decode the message contents into
- */
-static inline void mavlink_msg_setpoint_6dof_decode(const mavlink_message_t* msg, mavlink_setpoint_6dof_t* setpoint_6dof)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	setpoint_6dof->trans_x = mavlink_msg_setpoint_6dof_get_trans_x(msg);
-	setpoint_6dof->trans_y = mavlink_msg_setpoint_6dof_get_trans_y(msg);
-	setpoint_6dof->trans_z = mavlink_msg_setpoint_6dof_get_trans_z(msg);
-	setpoint_6dof->rot_x = mavlink_msg_setpoint_6dof_get_rot_x(msg);
-	setpoint_6dof->rot_y = mavlink_msg_setpoint_6dof_get_rot_y(msg);
-	setpoint_6dof->rot_z = mavlink_msg_setpoint_6dof_get_rot_z(msg);
-	setpoint_6dof->target_system = mavlink_msg_setpoint_6dof_get_target_system(msg);
-#else
-	memcpy(setpoint_6dof, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
-#endif
-}
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
deleted file mode 100644
index de80e4645..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h
+++ /dev/null
@@ -1,353 +0,0 @@
-// MESSAGE SETPOINT_8DOF PACKING
-
-#define MAVLINK_MSG_ID_SETPOINT_8DOF 148
-
-typedef struct __mavlink_setpoint_8dof_t
-{
- float val1; ///< Value 1
- float val2; ///< Value 2
- float val3; ///< Value 3
- float val4; ///< Value 4
- float val5; ///< Value 5
- float val6; ///< Value 6
- float val7; ///< Value 7
- float val8; ///< Value 8
- uint8_t target_system; ///< System ID
-} mavlink_setpoint_8dof_t;
-
-#define MAVLINK_MSG_ID_SETPOINT_8DOF_LEN 33
-#define MAVLINK_MSG_ID_148_LEN 33
-
-#define MAVLINK_MSG_ID_SETPOINT_8DOF_CRC 241
-#define MAVLINK_MSG_ID_148_CRC 241
-
-
-
-#define MAVLINK_MESSAGE_INFO_SETPOINT_8DOF { \
-	"SETPOINT_8DOF", \
-	9, \
-	{  { "val1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_setpoint_8dof_t, val1) }, \
-         { "val2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_setpoint_8dof_t, val2) }, \
-         { "val3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_setpoint_8dof_t, val3) }, \
-         { "val4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_setpoint_8dof_t, val4) }, \
-         { "val5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_setpoint_8dof_t, val5) }, \
-         { "val6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_setpoint_8dof_t, val6) }, \
-         { "val7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_setpoint_8dof_t, val7) }, \
-         { "val8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_setpoint_8dof_t, val8) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_setpoint_8dof_t, target_system) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a setpoint_8dof 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 target_system System ID
- * @param val1 Value 1
- * @param val2 Value 2
- * @param val3 Value 3
- * @param val4 Value 4
- * @param val5 Value 5
- * @param val6 Value 6
- * @param val7 Value 7
- * @param val8 Value 8
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
-#else
-	mavlink_setpoint_8dof_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
-#endif
-}
-
-/**
- * @brief Pack a setpoint_8dof 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 target_system System ID
- * @param val1 Value 1
- * @param val2 Value 2
- * @param val3 Value 3
- * @param val4 Value 4
- * @param val5 Value 5
- * @param val6 Value 6
- * @param val7 Value 7
- * @param val8 Value 8
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
-#else
-	mavlink_setpoint_8dof_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SETPOINT_8DOF;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
-#endif
-}
-
-/**
- * @brief Encode a setpoint_8dof 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 setpoint_8dof C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof)
-{
-	return mavlink_msg_setpoint_8dof_pack(system_id, component_id, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8);
-}
-
-/**
- * @brief Encode a setpoint_8dof 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 setpoint_8dof C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_setpoint_8dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof)
-{
-	return mavlink_msg_setpoint_8dof_pack_chan(system_id, component_id, chan, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8);
-}
-
-/**
- * @brief Send a setpoint_8dof message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param val1 Value 1
- * @param val2 Value 2
- * @param val3 Value 3
- * @param val4 Value 4
- * @param val5 Value 5
- * @param val6 Value 6
- * @param val7 Value 7
- * @param val8 Value 8
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_setpoint_8dof_send(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[MAVLINK_MSG_ID_SETPOINT_8DOF_LEN];
-	_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;
-	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
-
-// MESSAGE SETPOINT_8DOF UNPACKING
-
-
-/**
- * @brief Get field target_system from setpoint_8dof message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_setpoint_8dof_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  32);
-}
-
-/**
- * @brief Get field val1 from setpoint_8dof message
- *
- * @return Value 1
- */
-static inline float mavlink_msg_setpoint_8dof_get_val1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field val2 from setpoint_8dof message
- *
- * @return Value 2
- */
-static inline float mavlink_msg_setpoint_8dof_get_val2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field val3 from setpoint_8dof message
- *
- * @return Value 3
- */
-static inline float mavlink_msg_setpoint_8dof_get_val3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field val4 from setpoint_8dof message
- *
- * @return Value 4
- */
-static inline float mavlink_msg_setpoint_8dof_get_val4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field val5 from setpoint_8dof message
- *
- * @return Value 5
- */
-static inline float mavlink_msg_setpoint_8dof_get_val5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field val6 from setpoint_8dof message
- *
- * @return Value 6
- */
-static inline float mavlink_msg_setpoint_8dof_get_val6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field val7 from setpoint_8dof message
- *
- * @return Value 7
- */
-static inline float mavlink_msg_setpoint_8dof_get_val7(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field val8 from setpoint_8dof message
- *
- * @return Value 8
- */
-static inline float mavlink_msg_setpoint_8dof_get_val8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Decode a setpoint_8dof message into a struct
- *
- * @param msg The message to decode
- * @param setpoint_8dof C-struct to decode the message contents into
- */
-static inline void mavlink_msg_setpoint_8dof_decode(const mavlink_message_t* msg, mavlink_setpoint_8dof_t* setpoint_8dof)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	setpoint_8dof->val1 = mavlink_msg_setpoint_8dof_get_val1(msg);
-	setpoint_8dof->val2 = mavlink_msg_setpoint_8dof_get_val2(msg);
-	setpoint_8dof->val3 = mavlink_msg_setpoint_8dof_get_val3(msg);
-	setpoint_8dof->val4 = mavlink_msg_setpoint_8dof_get_val4(msg);
-	setpoint_8dof->val5 = mavlink_msg_setpoint_8dof_get_val5(msg);
-	setpoint_8dof->val6 = mavlink_msg_setpoint_8dof_get_val6(msg);
-	setpoint_8dof->val7 = mavlink_msg_setpoint_8dof_get_val7(msg);
-	setpoint_8dof->val8 = mavlink_msg_setpoint_8dof_get_val8(msg);
-	setpoint_8dof->target_system = mavlink_msg_setpoint_8dof_get_target_system(msg);
-#else
-	memcpy(setpoint_8dof, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
-#endif
-}
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
deleted file mode 100644
index ebd657cf3..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h
+++ /dev/null
@@ -1,617 +0,0 @@
-// MESSAGE SIM_STATE PACKING
-
-#define MAVLINK_MSG_ID_SIM_STATE 108
-
-typedef struct __mavlink_sim_state_t
-{
- float q1; ///< True attitude quaternion component 1
- float q2; ///< True attitude quaternion component 2
- float q3; ///< True attitude quaternion component 3
- float q4; ///< True attitude quaternion component 4
- float roll; ///< Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
- float pitch; ///< Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
- float yaw; ///< Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
- float xacc; ///< X acceleration m/s/s
- float yacc; ///< Y acceleration m/s/s
- float zacc; ///< Z acceleration m/s/s
- float xgyro; ///< Angular speed around X axis rad/s
- float ygyro; ///< Angular speed around Y axis rad/s
- float zgyro; ///< Angular speed around Z axis rad/s
- float lat; ///< Latitude in degrees
- float lon; ///< Longitude in degrees
- float alt; ///< Altitude in meters
- float std_dev_horz; ///< Horizontal position standard deviation
- float std_dev_vert; ///< Vertical position standard deviation
- float vn; ///< True velocity in m/s in NORTH direction in earth-fixed NED frame
- float ve; ///< True velocity in m/s in EAST direction in earth-fixed NED frame
- float vd; ///< True velocity in m/s in DOWN direction in earth-fixed NED frame
-} mavlink_sim_state_t;
-
-#define MAVLINK_MSG_ID_SIM_STATE_LEN 84
-#define MAVLINK_MSG_ID_108_LEN 84
-
-#define MAVLINK_MSG_ID_SIM_STATE_CRC 32
-#define MAVLINK_MSG_ID_108_CRC 32
-
-
-
-#define MAVLINK_MESSAGE_INFO_SIM_STATE { \
-	"SIM_STATE", \
-	21, \
-	{  { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sim_state_t, q1) }, \
-         { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sim_state_t, q2) }, \
-         { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sim_state_t, q3) }, \
-         { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sim_state_t, q4) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sim_state_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sim_state_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sim_state_t, yaw) }, \
-         { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sim_state_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sim_state_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_sim_state_t, zacc) }, \
-         { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_sim_state_t, xgyro) }, \
-         { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_sim_state_t, ygyro) }, \
-         { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_sim_state_t, zgyro) }, \
-         { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_sim_state_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_sim_state_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_sim_state_t, alt) }, \
-         { "std_dev_horz", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_sim_state_t, std_dev_horz) }, \
-         { "std_dev_vert", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_sim_state_t, std_dev_vert) }, \
-         { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_sim_state_t, vn) }, \
-         { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_sim_state_t, ve) }, \
-         { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_sim_state_t, vd) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a sim_state 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 q1 True attitude quaternion component 1
- * @param q2 True attitude quaternion component 2
- * @param q3 True attitude quaternion component 3
- * @param q4 True attitude quaternion component 4
- * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
- * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
- * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees
- * @param lon Longitude in degrees
- * @param alt Altitude in meters
- * @param std_dev_horz Horizontal position standard deviation
- * @param std_dev_vert Vertical position standard deviation
- * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame
- * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame
- * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SIM_STATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN);
-#else
-	mavlink_sim_state_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SIM_STATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIM_STATE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a sim_state 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 q1 True attitude quaternion component 1
- * @param q2 True attitude quaternion component 2
- * @param q3 True attitude quaternion component 3
- * @param q4 True attitude quaternion component 4
- * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
- * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
- * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees
- * @param lon Longitude in degrees
- * @param alt Altitude in meters
- * @param std_dev_horz Horizontal position standard deviation
- * @param std_dev_vert Vertical position standard deviation
- * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame
- * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame
- * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SIM_STATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN);
-#else
-	mavlink_sim_state_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SIM_STATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIM_STATE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a sim_state 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 sim_state C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state)
-{
-	return mavlink_msg_sim_state_pack(system_id, component_id, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd);
-}
-
-/**
- * @brief Encode a sim_state 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 sim_state C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state)
-{
-	return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd);
-}
-
-/**
- * @brief Send a sim_state message
- * @param chan MAVLink channel to send the message
- *
- * @param q1 True attitude quaternion component 1
- * @param q2 True attitude quaternion component 2
- * @param q3 True attitude quaternion component 3
- * @param q4 True attitude quaternion component 4
- * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
- * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
- * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees
- * @param lon Longitude in degrees
- * @param alt Altitude in meters
- * @param std_dev_horz Horizontal position standard deviation
- * @param std_dev_vert Vertical position standard deviation
- * @param vn True velocity in m/s in NORTH direction in earth-fixed NED frame
- * @param ve True velocity in m/s in EAST direction in earth-fixed NED frame
- * @param vd True velocity in m/s in DOWN direction in earth-fixed NED frame
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_sim_state_send(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[MAVLINK_MSG_ID_SIM_STATE_LEN];
-	_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;
-	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
-
-// MESSAGE SIM_STATE UNPACKING
-
-
-/**
- * @brief Get field q1 from sim_state message
- *
- * @return True attitude quaternion component 1
- */
-static inline float mavlink_msg_sim_state_get_q1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field q2 from sim_state message
- *
- * @return True attitude quaternion component 2
- */
-static inline float mavlink_msg_sim_state_get_q2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field q3 from sim_state message
- *
- * @return True attitude quaternion component 3
- */
-static inline float mavlink_msg_sim_state_get_q3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field q4 from sim_state message
- *
- * @return True attitude quaternion component 4
- */
-static inline float mavlink_msg_sim_state_get_q4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field roll from sim_state message
- *
- * @return Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
- */
-static inline float mavlink_msg_sim_state_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field pitch from sim_state message
- *
- * @return Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
- */
-static inline float mavlink_msg_sim_state_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field yaw from sim_state message
- *
- * @return Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
- */
-static inline float mavlink_msg_sim_state_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field xacc from sim_state message
- *
- * @return X acceleration m/s/s
- */
-static inline float mavlink_msg_sim_state_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field yacc from sim_state message
- *
- * @return Y acceleration m/s/s
- */
-static inline float mavlink_msg_sim_state_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field zacc from sim_state message
- *
- * @return Z acceleration m/s/s
- */
-static inline float mavlink_msg_sim_state_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  36);
-}
-
-/**
- * @brief Get field xgyro from sim_state message
- *
- * @return Angular speed around X axis rad/s
- */
-static inline float mavlink_msg_sim_state_get_xgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  40);
-}
-
-/**
- * @brief Get field ygyro from sim_state message
- *
- * @return Angular speed around Y axis rad/s
- */
-static inline float mavlink_msg_sim_state_get_ygyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  44);
-}
-
-/**
- * @brief Get field zgyro from sim_state message
- *
- * @return Angular speed around Z axis rad/s
- */
-static inline float mavlink_msg_sim_state_get_zgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  48);
-}
-
-/**
- * @brief Get field lat from sim_state message
- *
- * @return Latitude in degrees
- */
-static inline float mavlink_msg_sim_state_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  52);
-}
-
-/**
- * @brief Get field lon from sim_state message
- *
- * @return Longitude in degrees
- */
-static inline float mavlink_msg_sim_state_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  56);
-}
-
-/**
- * @brief Get field alt from sim_state message
- *
- * @return Altitude in meters
- */
-static inline float mavlink_msg_sim_state_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  60);
-}
-
-/**
- * @brief Get field std_dev_horz from sim_state message
- *
- * @return Horizontal position standard deviation
- */
-static inline float mavlink_msg_sim_state_get_std_dev_horz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  64);
-}
-
-/**
- * @brief Get field std_dev_vert from sim_state message
- *
- * @return Vertical position standard deviation
- */
-static inline float mavlink_msg_sim_state_get_std_dev_vert(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  68);
-}
-
-/**
- * @brief Get field vn from sim_state message
- *
- * @return True velocity in m/s in NORTH direction in earth-fixed NED frame
- */
-static inline float mavlink_msg_sim_state_get_vn(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  72);
-}
-
-/**
- * @brief Get field ve from sim_state message
- *
- * @return True velocity in m/s in EAST direction in earth-fixed NED frame
- */
-static inline float mavlink_msg_sim_state_get_ve(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  76);
-}
-
-/**
- * @brief Get field vd from sim_state message
- *
- * @return True velocity in m/s in DOWN direction in earth-fixed NED frame
- */
-static inline float mavlink_msg_sim_state_get_vd(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  80);
-}
-
-/**
- * @brief Decode a sim_state message into a struct
- *
- * @param msg The message to decode
- * @param sim_state C-struct to decode the message contents into
- */
-static inline void mavlink_msg_sim_state_decode(const mavlink_message_t* msg, mavlink_sim_state_t* sim_state)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	sim_state->q1 = mavlink_msg_sim_state_get_q1(msg);
-	sim_state->q2 = mavlink_msg_sim_state_get_q2(msg);
-	sim_state->q3 = mavlink_msg_sim_state_get_q3(msg);
-	sim_state->q4 = mavlink_msg_sim_state_get_q4(msg);
-	sim_state->roll = mavlink_msg_sim_state_get_roll(msg);
-	sim_state->pitch = mavlink_msg_sim_state_get_pitch(msg);
-	sim_state->yaw = mavlink_msg_sim_state_get_yaw(msg);
-	sim_state->xacc = mavlink_msg_sim_state_get_xacc(msg);
-	sim_state->yacc = mavlink_msg_sim_state_get_yacc(msg);
-	sim_state->zacc = mavlink_msg_sim_state_get_zacc(msg);
-	sim_state->xgyro = mavlink_msg_sim_state_get_xgyro(msg);
-	sim_state->ygyro = mavlink_msg_sim_state_get_ygyro(msg);
-	sim_state->zgyro = mavlink_msg_sim_state_get_zgyro(msg);
-	sim_state->lat = mavlink_msg_sim_state_get_lat(msg);
-	sim_state->lon = mavlink_msg_sim_state_get_lon(msg);
-	sim_state->alt = mavlink_msg_sim_state_get_alt(msg);
-	sim_state->std_dev_horz = mavlink_msg_sim_state_get_std_dev_horz(msg);
-	sim_state->std_dev_vert = mavlink_msg_sim_state_get_std_dev_vert(msg);
-	sim_state->vn = mavlink_msg_sim_state_get_vn(msg);
-	sim_state->ve = mavlink_msg_sim_state_get_ve(msg);
-	sim_state->vd = mavlink_msg_sim_state_get_vd(msg);
-#else
-	memcpy(sim_state, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SIM_STATE_LEN);
-#endif
-}
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
deleted file mode 100644
index 2db627b96..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h
+++ /dev/null
@@ -1,353 +0,0 @@
-// MESSAGE STATE_CORRECTION PACKING
-
-#define MAVLINK_MSG_ID_STATE_CORRECTION 64
-
-typedef struct __mavlink_state_correction_t
-{
- float xErr; ///< x position error
- float yErr; ///< y position error
- float zErr; ///< z position error
- float rollErr; ///< roll error (radians)
- float pitchErr; ///< pitch error (radians)
- float yawErr; ///< yaw error (radians)
- float vxErr; ///< x velocity
- float vyErr; ///< y velocity
- float vzErr; ///< z velocity
-} mavlink_state_correction_t;
-
-#define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36
-#define MAVLINK_MSG_ID_64_LEN 36
-
-#define MAVLINK_MSG_ID_STATE_CORRECTION_CRC 130
-#define MAVLINK_MSG_ID_64_CRC 130
-
-
-
-#define MAVLINK_MESSAGE_INFO_STATE_CORRECTION { \
-	"STATE_CORRECTION", \
-	9, \
-	{  { "xErr", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_state_correction_t, xErr) }, \
-         { "yErr", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_state_correction_t, yErr) }, \
-         { "zErr", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_state_correction_t, zErr) }, \
-         { "rollErr", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_state_correction_t, rollErr) }, \
-         { "pitchErr", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_state_correction_t, pitchErr) }, \
-         { "yawErr", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_state_correction_t, yawErr) }, \
-         { "vxErr", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_state_correction_t, vxErr) }, \
-         { "vyErr", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_state_correction_t, vyErr) }, \
-         { "vzErr", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_state_correction_t, vzErr) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a state_correction 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 xErr x position error
- * @param yErr y position error
- * @param zErr z position error
- * @param rollErr roll error (radians)
- * @param pitchErr pitch error (radians)
- * @param yawErr yaw error (radians)
- * @param vxErr x velocity
- * @param vyErr y velocity
- * @param vzErr z velocity
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_STATE_CORRECTION_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
-#else
-	mavlink_state_correction_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
-#endif
-}
-
-/**
- * @brief Pack a state_correction 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 xErr x position error
- * @param yErr y position error
- * @param zErr z position error
- * @param rollErr roll error (radians)
- * @param pitchErr pitch error (radians)
- * @param yawErr yaw error (radians)
- * @param vxErr x velocity
- * @param vyErr y velocity
- * @param vzErr z velocity
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_STATE_CORRECTION_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
-#else
-	mavlink_state_correction_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
-#endif
-}
-
-/**
- * @brief Encode a state_correction 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 state_correction C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction)
-{
-	return mavlink_msg_state_correction_pack(system_id, component_id, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr);
-}
-
-/**
- * @brief Encode a state_correction 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 state_correction C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_state_correction_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction)
-{
-	return mavlink_msg_state_correction_pack_chan(system_id, component_id, chan, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr);
-}
-
-/**
- * @brief Send a state_correction message
- * @param chan MAVLink channel to send the message
- *
- * @param xErr x position error
- * @param yErr y position error
- * @param zErr z position error
- * @param rollErr roll error (radians)
- * @param pitchErr pitch error (radians)
- * @param yawErr yaw error (radians)
- * @param vxErr x velocity
- * @param vyErr y velocity
- * @param vzErr z velocity
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_state_correction_send(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[MAVLINK_MSG_ID_STATE_CORRECTION_LEN];
-	_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;
-	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
-
-// MESSAGE STATE_CORRECTION UNPACKING
-
-
-/**
- * @brief Get field xErr from state_correction message
- *
- * @return x position error
- */
-static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field yErr from state_correction message
- *
- * @return y position error
- */
-static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field zErr from state_correction message
- *
- * @return z position error
- */
-static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field rollErr from state_correction message
- *
- * @return roll error (radians)
- */
-static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field pitchErr from state_correction message
- *
- * @return pitch error (radians)
- */
-static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field yawErr from state_correction message
- *
- * @return yaw error (radians)
- */
-static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field vxErr from state_correction message
- *
- * @return x velocity
- */
-static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field vyErr from state_correction message
- *
- * @return y velocity
- */
-static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field vzErr from state_correction message
- *
- * @return z velocity
- */
-static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Decode a state_correction message into a struct
- *
- * @param msg The message to decode
- * @param state_correction C-struct to decode the message contents into
- */
-static inline void mavlink_msg_state_correction_decode(const mavlink_message_t* msg, mavlink_state_correction_t* state_correction)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	state_correction->xErr = mavlink_msg_state_correction_get_xErr(msg);
-	state_correction->yErr = mavlink_msg_state_correction_get_yErr(msg);
-	state_correction->zErr = mavlink_msg_state_correction_get_zErr(msg);
-	state_correction->rollErr = mavlink_msg_state_correction_get_rollErr(msg);
-	state_correction->pitchErr = mavlink_msg_state_correction_get_pitchErr(msg);
-	state_correction->yawErr = mavlink_msg_state_correction_get_yawErr(msg);
-	state_correction->vxErr = mavlink_msg_state_correction_get_vxErr(msg);
-	state_correction->vyErr = mavlink_msg_state_correction_get_vyErr(msg);
-	state_correction->vzErr = mavlink_msg_state_correction_get_vzErr(msg);
-#else
-	memcpy(state_correction, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h
deleted file mode 100644
index 536ca0634..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h
+++ /dev/null
@@ -1,193 +0,0 @@
-// MESSAGE STATUSTEXT PACKING
-
-#define MAVLINK_MSG_ID_STATUSTEXT 253
-
-typedef struct __mavlink_statustext_t
-{
- uint8_t severity; ///< Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.
- char text[50]; ///< Status text message, without null termination character
-} mavlink_statustext_t;
-
-#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51
-#define MAVLINK_MSG_ID_253_LEN 51
-
-#define MAVLINK_MSG_ID_STATUSTEXT_CRC 83
-#define MAVLINK_MSG_ID_253_CRC 83
-
-#define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50
-
-#define MAVLINK_MESSAGE_INFO_STATUSTEXT { \
-	"STATUSTEXT", \
-	2, \
-	{  { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \
-         { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a statustext 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 severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.
- * @param text Status text message, without null termination character
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t severity, const char *text)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN];
-	_mav_put_uint8_t(buf, 0, severity);
-	_mav_put_char_array(buf, 1, text, 50);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN);
-#else
-	mavlink_statustext_t packet;
-	packet.severity = severity;
-	mav_array_memcpy(packet.text, text, sizeof(char)*50);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_STATUSTEXT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a statustext 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 severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.
- * @param text Status text message, without null termination character
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t severity,const char *text)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN];
-	_mav_put_uint8_t(buf, 0, severity);
-	_mav_put_char_array(buf, 1, text, 50);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN);
-#else
-	mavlink_statustext_t packet;
-	packet.severity = severity;
-	mav_array_memcpy(packet.text, text, sizeof(char)*50);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_STATUSTEXT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a statustext 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 statustext C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_statustext_t* statustext)
-{
-	return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text);
-}
-
-/**
- * @brief Encode a statustext 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 statustext C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext)
-{
-	return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text);
-}
-
-/**
- * @brief Send a statustext message
- * @param chan MAVLink channel to send the message
- *
- * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.
- * @param text Status text message, without null termination character
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN];
-	_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;
-	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
-
-// MESSAGE STATUSTEXT UNPACKING
-
-
-/**
- * @brief Get field severity from statustext message
- *
- * @return Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.
- */
-static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field text from statustext message
- *
- * @return Status text message, without null termination character
- */
-static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char *text)
-{
-	return _MAV_RETURN_char_array(msg, text, 50,  1);
-}
-
-/**
- * @brief Decode a statustext message into a struct
- *
- * @param msg The message to decode
- * @param statustext C-struct to decode the message contents into
- */
-static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	statustext->severity = mavlink_msg_statustext_get_severity(msg);
-	mavlink_msg_statustext_get_text(msg, statustext->text);
-#else
-	memcpy(statustext, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_STATUSTEXT_LEN);
-#endif
-}
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
deleted file mode 100644
index 101b36678..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
+++ /dev/null
@@ -1,441 +0,0 @@
-// MESSAGE SYS_STATUS PACKING
-
-#define MAVLINK_MSG_ID_SYS_STATUS 1
-
-typedef struct __mavlink_sys_status_t
-{
- uint32_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- uint32_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- uint32_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
- uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt)
- int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- uint16_t drop_rate_comm; ///< Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- uint16_t errors_comm; ///< Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- uint16_t errors_count1; ///< Autopilot-specific errors
- uint16_t errors_count2; ///< Autopilot-specific errors
- uint16_t errors_count3; ///< Autopilot-specific errors
- uint16_t errors_count4; ///< Autopilot-specific errors
- int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
-} mavlink_sys_status_t;
-
-#define MAVLINK_MSG_ID_SYS_STATUS_LEN 31
-#define MAVLINK_MSG_ID_1_LEN 31
-
-#define MAVLINK_MSG_ID_SYS_STATUS_CRC 124
-#define MAVLINK_MSG_ID_1_CRC 124
-
-
-
-#define MAVLINK_MESSAGE_INFO_SYS_STATUS { \
-	"SYS_STATUS", \
-	13, \
-	{  { "onboard_control_sensors_present", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \
-         { "onboard_control_sensors_enabled", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \
-         { "onboard_control_sensors_health", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \
-         { "load", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_sys_status_t, load) }, \
-         { "voltage_battery", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_sys_status_t, voltage_battery) }, \
-         { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_sys_status_t, current_battery) }, \
-         { "drop_rate_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_sys_status_t, drop_rate_comm) }, \
-         { "errors_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_sys_status_t, errors_comm) }, \
-         { "errors_count1", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_sys_status_t, errors_count1) }, \
-         { "errors_count2", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count2) }, \
-         { "errors_count3", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count3) }, \
-         { "errors_count4", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count4) }, \
-         { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 30, offsetof(mavlink_sys_status_t, battery_remaining) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a sys_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 onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
- * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
- * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
- * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- * @param errors_count1 Autopilot-specific errors
- * @param errors_count2 Autopilot-specific errors
- * @param errors_count3 Autopilot-specific errors
- * @param errors_count4 Autopilot-specific errors
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SYS_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN);
-#else
-	mavlink_sys_status_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a sys_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 onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
- * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
- * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
- * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- * @param errors_count1 Autopilot-specific errors
- * @param errors_count2 Autopilot-specific errors
- * @param errors_count3 Autopilot-specific errors
- * @param errors_count4 Autopilot-specific errors
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SYS_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN);
-#else
-	mavlink_sys_status_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a sys_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 sys_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status)
-{
-	return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4);
-}
-
-/**
- * @brief Encode a sys_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 sys_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status)
-{
-	return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4);
-}
-
-/**
- * @brief Send a sys_status message
- * @param chan MAVLink channel to send the message
- *
- * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
- * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
- * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
- * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- * @param errors_count1 Autopilot-specific errors
- * @param errors_count2 Autopilot-specific errors
- * @param errors_count3 Autopilot-specific errors
- * @param errors_count4 Autopilot-specific errors
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_sys_status_send(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[MAVLINK_MSG_ID_SYS_STATUS_LEN];
-	_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;
-	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
-
-// MESSAGE SYS_STATUS UNPACKING
-
-
-/**
- * @brief Get field onboard_control_sensors_present from sys_status message
- *
- * @return Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- */
-static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field onboard_control_sensors_enabled from sys_status message
- *
- * @return Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- */
-static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  4);
-}
-
-/**
- * @brief Get field onboard_control_sensors_health from sys_status message
- *
- * @return Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
- */
-static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  8);
-}
-
-/**
- * @brief Get field load from sys_status message
- *
- * @return Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
- */
-static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field voltage_battery from sys_status message
- *
- * @return Battery voltage, in millivolts (1 = 1 millivolt)
- */
-static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  14);
-}
-
-/**
- * @brief Get field current_battery from sys_status message
- *
- * @return Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
- */
-static inline int16_t mavlink_msg_sys_status_get_current_battery(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  16);
-}
-
-/**
- * @brief Get field battery_remaining from sys_status message
- *
- * @return Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
- */
-static inline int8_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int8_t(msg,  30);
-}
-
-/**
- * @brief Get field drop_rate_comm from sys_status message
- *
- * @return Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- */
-static inline uint16_t mavlink_msg_sys_status_get_drop_rate_comm(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  18);
-}
-
-/**
- * @brief Get field errors_comm from sys_status message
- *
- * @return Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
- */
-static inline uint16_t mavlink_msg_sys_status_get_errors_comm(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  20);
-}
-
-/**
- * @brief Get field errors_count1 from sys_status message
- *
- * @return Autopilot-specific errors
- */
-static inline uint16_t mavlink_msg_sys_status_get_errors_count1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  22);
-}
-
-/**
- * @brief Get field errors_count2 from sys_status message
- *
- * @return Autopilot-specific errors
- */
-static inline uint16_t mavlink_msg_sys_status_get_errors_count2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  24);
-}
-
-/**
- * @brief Get field errors_count3 from sys_status message
- *
- * @return Autopilot-specific errors
- */
-static inline uint16_t mavlink_msg_sys_status_get_errors_count3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  26);
-}
-
-/**
- * @brief Get field errors_count4 from sys_status message
- *
- * @return Autopilot-specific errors
- */
-static inline uint16_t mavlink_msg_sys_status_get_errors_count4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  28);
-}
-
-/**
- * @brief Decode a sys_status message into a struct
- *
- * @param msg The message to decode
- * @param sys_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	sys_status->onboard_control_sensors_present = mavlink_msg_sys_status_get_onboard_control_sensors_present(msg);
-	sys_status->onboard_control_sensors_enabled = mavlink_msg_sys_status_get_onboard_control_sensors_enabled(msg);
-	sys_status->onboard_control_sensors_health = mavlink_msg_sys_status_get_onboard_control_sensors_health(msg);
-	sys_status->load = mavlink_msg_sys_status_get_load(msg);
-	sys_status->voltage_battery = mavlink_msg_sys_status_get_voltage_battery(msg);
-	sys_status->current_battery = mavlink_msg_sys_status_get_current_battery(msg);
-	sys_status->drop_rate_comm = mavlink_msg_sys_status_get_drop_rate_comm(msg);
-	sys_status->errors_comm = mavlink_msg_sys_status_get_errors_comm(msg);
-	sys_status->errors_count1 = mavlink_msg_sys_status_get_errors_count1(msg);
-	sys_status->errors_count2 = mavlink_msg_sys_status_get_errors_count2(msg);
-	sys_status->errors_count3 = mavlink_msg_sys_status_get_errors_count3(msg);
-	sys_status->errors_count4 = mavlink_msg_sys_status_get_errors_count4(msg);
-	sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg);
-#else
-	memcpy(sys_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYS_STATUS_LEN);
-#endif
-}
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
deleted file mode 100644
index 1807567ae..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE SYSTEM_TIME PACKING
-
-#define MAVLINK_MSG_ID_SYSTEM_TIME 2
-
-typedef struct __mavlink_system_time_t
-{
- uint64_t time_unix_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch.
- uint32_t time_boot_ms; ///< Timestamp of the component clock since boot time in milliseconds.
-} mavlink_system_time_t;
-
-#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 12
-#define MAVLINK_MSG_ID_2_LEN 12
-
-#define MAVLINK_MSG_ID_SYSTEM_TIME_CRC 137
-#define MAVLINK_MSG_ID_2_CRC 137
-
-
-
-#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME { \
-	"SYSTEM_TIME", \
-	2, \
-	{  { "time_unix_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_system_time_t, time_unix_usec) }, \
-         { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_system_time_t, time_boot_ms) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a system_time 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_unix_usec Timestamp of the master clock in microseconds since UNIX epoch.
- * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint64_t time_unix_usec, uint32_t time_boot_ms)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN];
-	_mav_put_uint64_t(buf, 0, time_unix_usec);
-	_mav_put_uint32_t(buf, 8, time_boot_ms);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
-#else
-	mavlink_system_time_t packet;
-	packet.time_unix_usec = time_unix_usec;
-	packet.time_boot_ms = time_boot_ms;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
-#endif
-}
-
-/**
- * @brief Pack a system_time 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_unix_usec Timestamp of the master clock in microseconds since UNIX epoch.
- * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint64_t time_unix_usec,uint32_t time_boot_ms)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN];
-	_mav_put_uint64_t(buf, 0, time_unix_usec);
-	_mav_put_uint32_t(buf, 8, time_boot_ms);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
-#else
-	mavlink_system_time_t packet;
-	packet.time_unix_usec = time_unix_usec;
-	packet.time_boot_ms = time_boot_ms;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
-#endif
-}
-
-/**
- * @brief Encode a system_time 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 system_time C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_system_time_t* system_time)
-{
-	return mavlink_msg_system_time_pack(system_id, component_id, msg, system_time->time_unix_usec, system_time->time_boot_ms);
-}
-
-/**
- * @brief Encode a system_time 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 system_time C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_system_time_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_system_time_t* system_time)
-{
-	return mavlink_msg_system_time_pack_chan(system_id, component_id, chan, msg, system_time->time_unix_usec, system_time->time_boot_ms);
-}
-
-/**
- * @brief Send a system_time message
- * @param chan MAVLink channel to send the message
- *
- * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch.
- * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds.
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN];
-	_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;
-	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
-
-// MESSAGE SYSTEM_TIME UNPACKING
-
-
-/**
- * @brief Get field time_unix_usec from system_time message
- *
- * @return Timestamp of the master clock in microseconds since UNIX epoch.
- */
-static inline uint64_t mavlink_msg_system_time_get_time_unix_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field time_boot_ms from system_time message
- *
- * @return Timestamp of the component clock since boot time in milliseconds.
- */
-static inline uint32_t mavlink_msg_system_time_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  8);
-}
-
-/**
- * @brief Decode a system_time message into a struct
- *
- * @param msg The message to decode
- * @param system_time C-struct to decode the message contents into
- */
-static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, mavlink_system_time_t* system_time)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	system_time->time_unix_usec = mavlink_msg_system_time_get_time_unix_usec(msg);
-	system_time->time_boot_ms = mavlink_msg_system_time_get_time_boot_ms(msg);
-#else
-	memcpy(system_time, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
-#endif
-}
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
deleted file mode 100644
index 5b1093a3d..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE VFR_HUD PACKING
-
-#define MAVLINK_MSG_ID_VFR_HUD 74
-
-typedef struct __mavlink_vfr_hud_t
-{
- float airspeed; ///< Current airspeed in m/s
- float groundspeed; ///< Current ground speed in m/s
- float alt; ///< Current altitude (MSL), in meters
- float climb; ///< Current climb rate in meters/second
- int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north)
- uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100
-} mavlink_vfr_hud_t;
-
-#define MAVLINK_MSG_ID_VFR_HUD_LEN 20
-#define MAVLINK_MSG_ID_74_LEN 20
-
-#define MAVLINK_MSG_ID_VFR_HUD_CRC 20
-#define MAVLINK_MSG_ID_74_CRC 20
-
-
-
-#define MAVLINK_MESSAGE_INFO_VFR_HUD { \
-	"VFR_HUD", \
-	6, \
-	{  { "airspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_vfr_hud_t, airspeed) }, \
-         { "groundspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_vfr_hud_t, groundspeed) }, \
-         { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vfr_hud_t, alt) }, \
-         { "climb", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vfr_hud_t, climb) }, \
-         { "heading", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_vfr_hud_t, heading) }, \
-         { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_vfr_hud_t, throttle) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a vfr_hud 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 airspeed Current airspeed in m/s
- * @param groundspeed Current ground speed in m/s
- * @param heading Current heading in degrees, in compass units (0..360, 0=north)
- * @param throttle Current throttle setting in integer percent, 0 to 100
- * @param alt Current altitude (MSL), in meters
- * @param climb Current climb rate in meters/second
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_VFR_HUD_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN);
-#else
-	mavlink_vfr_hud_t packet;
-	packet.airspeed = airspeed;
-	packet.groundspeed = groundspeed;
-	packet.alt = alt;
-	packet.climb = climb;
-	packet.heading = heading;
-	packet.throttle = throttle;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_VFR_HUD;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN);
-#endif
-}
-
-/**
- * @brief Pack a vfr_hud 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 airspeed Current airspeed in m/s
- * @param groundspeed Current ground speed in m/s
- * @param heading Current heading in degrees, in compass units (0..360, 0=north)
- * @param throttle Current throttle setting in integer percent, 0 to 100
- * @param alt Current altitude (MSL), in meters
- * @param climb Current climb rate in meters/second
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float airspeed,float groundspeed,int16_t heading,uint16_t throttle,float alt,float climb)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_VFR_HUD_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN);
-#else
-	mavlink_vfr_hud_t packet;
-	packet.airspeed = airspeed;
-	packet.groundspeed = groundspeed;
-	packet.alt = alt;
-	packet.climb = climb;
-	packet.heading = heading;
-	packet.throttle = throttle;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_VFR_HUD;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN);
-#endif
-}
-
-/**
- * @brief Encode a vfr_hud 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 vfr_hud C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud)
-{
-	return mavlink_msg_vfr_hud_pack(system_id, component_id, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb);
-}
-
-/**
- * @brief Encode a vfr_hud 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 vfr_hud C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_vfr_hud_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud)
-{
-	return mavlink_msg_vfr_hud_pack_chan(system_id, component_id, chan, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb);
-}
-
-/**
- * @brief Send a vfr_hud message
- * @param chan MAVLink channel to send the message
- *
- * @param airspeed Current airspeed in m/s
- * @param groundspeed Current ground speed in m/s
- * @param heading Current heading in degrees, in compass units (0..360, 0=north)
- * @param throttle Current throttle setting in integer percent, 0 to 100
- * @param alt Current altitude (MSL), in meters
- * @param climb Current climb rate in meters/second
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_vfr_hud_send(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[MAVLINK_MSG_ID_VFR_HUD_LEN];
-	_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;
-	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
-
-// MESSAGE VFR_HUD UNPACKING
-
-
-/**
- * @brief Get field airspeed from vfr_hud message
- *
- * @return Current airspeed in m/s
- */
-static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field groundspeed from vfr_hud message
- *
- * @return Current ground speed in m/s
- */
-static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field heading from vfr_hud message
- *
- * @return Current heading in degrees, in compass units (0..360, 0=north)
- */
-static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  16);
-}
-
-/**
- * @brief Get field throttle from vfr_hud message
- *
- * @return Current throttle setting in integer percent, 0 to 100
- */
-static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  18);
-}
-
-/**
- * @brief Get field alt from vfr_hud message
- *
- * @return Current altitude (MSL), in meters
- */
-static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field climb from vfr_hud message
- *
- * @return Current climb rate in meters/second
- */
-static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Decode a vfr_hud message into a struct
- *
- * @param msg The message to decode
- * @param vfr_hud C-struct to decode the message contents into
- */
-static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavlink_vfr_hud_t* vfr_hud)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	vfr_hud->airspeed = mavlink_msg_vfr_hud_get_airspeed(msg);
-	vfr_hud->groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg);
-	vfr_hud->alt = mavlink_msg_vfr_hud_get_alt(msg);
-	vfr_hud->climb = mavlink_msg_vfr_hud_get_climb(msg);
-	vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg);
-	vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg);
-#else
-	memcpy(vfr_hud, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VFR_HUD_LEN);
-#endif
-}
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
deleted file mode 100644
index a254202e4..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE VICON_POSITION_ESTIMATE PACKING
-
-#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 104
-
-typedef struct __mavlink_vicon_position_estimate_t
-{
- uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
- float x; ///< Global X position
- float y; ///< Global Y position
- float z; ///< Global Z position
- float roll; ///< Roll angle in rad
- float pitch; ///< Pitch angle in rad
- float yaw; ///< Yaw angle in rad
-} mavlink_vicon_position_estimate_t;
-
-#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32
-#define MAVLINK_MSG_ID_104_LEN 32
-
-#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC 56
-#define MAVLINK_MSG_ID_104_CRC 56
-
-
-
-#define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \
-	"VICON_POSITION_ESTIMATE", \
-	7, \
-	{  { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \
-         { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vicon_position_estimate_t, z) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a vicon_position_estimate 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
-#else
-	mavlink_vicon_position_estimate_t packet;
-	packet.usec = usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a vicon_position_estimate 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
-#else
-	mavlink_vicon_position_estimate_t packet;
-	packet.usec = usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a vicon_position_estimate 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 vicon_position_estimate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate)
-{
-	return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw);
-}
-
-/**
- * @brief Encode a vicon_position_estimate 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 vicon_position_estimate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate)
-{
-	return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw);
-}
-
-/**
- * @brief Send a vicon_position_estimate message
- * @param chan MAVLink channel to send the message
- *
- * @param usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_vicon_position_estimate_send(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[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN];
-	_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;
-	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
-
-// MESSAGE VICON_POSITION_ESTIMATE UNPACKING
-
-
-/**
- * @brief Get field usec from vicon_position_estimate message
- *
- * @return Timestamp (microseconds, synced to UNIX time or since system boot)
- */
-static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field x from vicon_position_estimate message
- *
- * @return Global X position
- */
-static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field y from vicon_position_estimate message
- *
- * @return Global Y position
- */
-static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field z from vicon_position_estimate message
- *
- * @return Global Z position
- */
-static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field roll from vicon_position_estimate message
- *
- * @return Roll angle in rad
- */
-static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field pitch from vicon_position_estimate message
- *
- * @return Pitch angle in rad
- */
-static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field yaw from vicon_position_estimate message
- *
- * @return Yaw angle in rad
- */
-static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Decode a vicon_position_estimate message into a struct
- *
- * @param msg The message to decode
- * @param vicon_position_estimate C-struct to decode the message contents into
- */
-static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_message_t* msg, mavlink_vicon_position_estimate_t* vicon_position_estimate)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	vicon_position_estimate->usec = mavlink_msg_vicon_position_estimate_get_usec(msg);
-	vicon_position_estimate->x = mavlink_msg_vicon_position_estimate_get_x(msg);
-	vicon_position_estimate->y = mavlink_msg_vicon_position_estimate_get_y(msg);
-	vicon_position_estimate->z = mavlink_msg_vicon_position_estimate_get_z(msg);
-	vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg);
-	vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg);
-	vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg);
-#else
-	memcpy(vicon_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
-#endif
-}
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
deleted file mode 100644
index f7a741b09..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE VISION_POSITION_ESTIMATE PACKING
-
-#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 102
-
-typedef struct __mavlink_vision_position_estimate_t
-{
- uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
- float x; ///< Global X position
- float y; ///< Global Y position
- float z; ///< Global Z position
- float roll; ///< Roll angle in rad
- float pitch; ///< Pitch angle in rad
- float yaw; ///< Yaw angle in rad
-} mavlink_vision_position_estimate_t;
-
-#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32
-#define MAVLINK_MSG_ID_102_LEN 32
-
-#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC 158
-#define MAVLINK_MSG_ID_102_CRC 158
-
-
-
-#define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \
-	"VISION_POSITION_ESTIMATE", \
-	7, \
-	{  { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \
-         { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_position_estimate_t, z) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a vision_position_estimate 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
-#else
-	mavlink_vision_position_estimate_t packet;
-	packet.usec = usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a vision_position_estimate 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
-#else
-	mavlink_vision_position_estimate_t packet;
-	packet.usec = usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a vision_position_estimate 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 vision_position_estimate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate)
-{
-	return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw);
-}
-
-/**
- * @brief Encode a vision_position_estimate 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 vision_position_estimate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate)
-{
-	return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw);
-}
-
-/**
- * @brief Send a vision_position_estimate message
- * @param chan MAVLink channel to send the message
- *
- * @param usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X position
- * @param y Global Y position
- * @param z Global Z position
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_vision_position_estimate_send(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[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN];
-	_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;
-	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
-
-// MESSAGE VISION_POSITION_ESTIMATE UNPACKING
-
-
-/**
- * @brief Get field usec from vision_position_estimate message
- *
- * @return Timestamp (microseconds, synced to UNIX time or since system boot)
- */
-static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field x from vision_position_estimate message
- *
- * @return Global X position
- */
-static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field y from vision_position_estimate message
- *
- * @return Global Y position
- */
-static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field z from vision_position_estimate message
- *
- * @return Global Z position
- */
-static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field roll from vision_position_estimate message
- *
- * @return Roll angle in rad
- */
-static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field pitch from vision_position_estimate message
- *
- * @return Pitch angle in rad
- */
-static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field yaw from vision_position_estimate message
- *
- * @return Yaw angle in rad
- */
-static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Decode a vision_position_estimate message into a struct
- *
- * @param msg The message to decode
- * @param vision_position_estimate C-struct to decode the message contents into
- */
-static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg);
-	vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg);
-	vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg);
-	vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg);
-	vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg);
-	vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg);
-	vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg);
-#else
-	memcpy(vision_position_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
-#endif
-}
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
deleted file mode 100644
index 660225128..000000000
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE VISION_SPEED_ESTIMATE PACKING
-
-#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 103
-
-typedef struct __mavlink_vision_speed_estimate_t
-{
- uint64_t usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
- float x; ///< Global X speed
- float y; ///< Global Y speed
- float z; ///< Global Z speed
-} mavlink_vision_speed_estimate_t;
-
-#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20
-#define MAVLINK_MSG_ID_103_LEN 20
-
-#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC 208
-#define MAVLINK_MSG_ID_103_CRC 208
-
-
-
-#define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \
-	"VISION_SPEED_ESTIMATE", \
-	4, \
-	{  { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \
-         { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a vision_speed_estimate 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X speed
- * @param y Global Y speed
- * @param z Global Z speed
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint64_t usec, float x, float y, float z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
-#else
-	mavlink_vision_speed_estimate_t packet;
-	packet.usec = usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a vision_speed_estimate 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 usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X speed
- * @param y Global Y speed
- * @param z Global Z speed
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint64_t usec,float x,float y,float z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
-#else
-	mavlink_vision_speed_estimate_t packet;
-	packet.usec = usec;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a vision_speed_estimate 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 vision_speed_estimate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate)
-{
-	return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z);
-}
-
-/**
- * @brief Encode a vision_speed_estimate 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 vision_speed_estimate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate)
-{
-	return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z);
-}
-
-/**
- * @brief Send a vision_speed_estimate message
- * @param chan MAVLink channel to send the message
- *
- * @param usec Timestamp (microseconds, synced to UNIX time or since system boot)
- * @param x Global X speed
- * @param y Global Y speed
- * @param z Global Z speed
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN];
-	_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;
-	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
-
-// MESSAGE VISION_SPEED_ESTIMATE UNPACKING
-
-
-/**
- * @brief Get field usec from vision_speed_estimate message
- *
- * @return Timestamp (microseconds, synced to UNIX time or since system boot)
- */
-static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field x from vision_speed_estimate message
- *
- * @return Global X speed
- */
-static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field y from vision_speed_estimate message
- *
- * @return Global Y speed
- */
-static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field z from vision_speed_estimate message
- *
- * @return Global Z speed
- */
-static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a vision_speed_estimate message into a struct
- *
- * @param msg The message to decode
- * @param vision_speed_estimate C-struct to decode the message contents into
- */
-static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg);
-	vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg);
-	vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg);
-	vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg);
-#else
-	memcpy(vision_speed_estimate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h
deleted file mode 100644
index c5aa9ddf3..000000000
--- a/mavlink/include/mavlink/v1.0/common/testsuite.h
+++ /dev/null
@@ -1,5628 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from common.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef COMMON_TESTSUITE_H
-#define COMMON_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-
-static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-
-	mavlink_test_common(system_id, component_id, last_msg);
-}
-#endif
-
-
-
-
-static void mavlink_test_heartbeat(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_heartbeat_t packet_in = {
-		963497464,
-	}17,
-	}84,
-	}151,
-	}218,
-	}3,
-	};
-	mavlink_heartbeat_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.custom_mode = packet_in.custom_mode;
-        	packet1.type = packet_in.type;
-        	packet1.autopilot = packet_in.autopilot;
-        	packet1.base_mode = packet_in.base_mode;
-        	packet1.system_status = packet_in.system_status;
-        	packet1.mavlink_version = packet_in.mavlink_version;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_heartbeat_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status );
-	mavlink_msg_heartbeat_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status );
-	mavlink_msg_heartbeat_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_heartbeat_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_heartbeat_send(MAVLINK_COMM_1 , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status );
-	mavlink_msg_heartbeat_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_sys_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_sys_status_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}18275,
-	}18379,
-	}18483,
-	}18587,
-	}18691,
-	}223,
-	};
-	mavlink_sys_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.onboard_control_sensors_present = packet_in.onboard_control_sensors_present;
-        	packet1.onboard_control_sensors_enabled = packet_in.onboard_control_sensors_enabled;
-        	packet1.onboard_control_sensors_health = packet_in.onboard_control_sensors_health;
-        	packet1.load = packet_in.load;
-        	packet1.voltage_battery = packet_in.voltage_battery;
-        	packet1.current_battery = packet_in.current_battery;
-        	packet1.drop_rate_comm = packet_in.drop_rate_comm;
-        	packet1.errors_comm = packet_in.errors_comm;
-        	packet1.errors_count1 = packet_in.errors_count1;
-        	packet1.errors_count2 = packet_in.errors_count2;
-        	packet1.errors_count3 = packet_in.errors_count3;
-        	packet1.errors_count4 = packet_in.errors_count4;
-        	packet1.battery_remaining = packet_in.battery_remaining;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sys_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_sys_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sys_status_pack(system_id, component_id, &msg , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 );
-	mavlink_msg_sys_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sys_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 );
-	mavlink_msg_sys_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_sys_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sys_status_send(MAVLINK_COMM_1 , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 );
-	mavlink_msg_sys_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_system_time(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_system_time_t packet_in = {
-		93372036854775807ULL,
-	}963497880,
-	};
-	mavlink_system_time_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_unix_usec = packet_in.time_unix_usec;
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_system_time_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_system_time_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_system_time_pack(system_id, component_id, &msg , packet1.time_unix_usec , packet1.time_boot_ms );
-	mavlink_msg_system_time_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_system_time_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_unix_usec , packet1.time_boot_ms );
-	mavlink_msg_system_time_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_system_time_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_system_time_send(MAVLINK_COMM_1 , packet1.time_unix_usec , packet1.time_boot_ms );
-	mavlink_msg_system_time_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_ping(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_ping_t packet_in = {
-		93372036854775807ULL,
-	}963497880,
-	}41,
-	}108,
-	};
-	mavlink_ping_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.seq = packet_in.seq;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ping_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_ping_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ping_pack(system_id, component_id, &msg , packet1.time_usec , packet1.seq , packet1.target_system , packet1.target_component );
-	mavlink_msg_ping_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ping_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.seq , packet1.target_system , packet1.target_component );
-	mavlink_msg_ping_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_ping_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ping_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.seq , packet1.target_system , packet1.target_component );
-	mavlink_msg_ping_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_change_operator_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_change_operator_control_t packet_in = {
-		5,
-	}72,
-	}139,
-	}"DEFGHIJKLMNOPQRSTUVWXYZA",
-	};
-	mavlink_change_operator_control_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.control_request = packet_in.control_request;
-        	packet1.version = packet_in.version;
-        
-        	mav_array_memcpy(packet1.passkey, packet_in.passkey, sizeof(char)*25);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_change_operator_control_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_change_operator_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_change_operator_control_pack(system_id, component_id, &msg , packet1.target_system , packet1.control_request , packet1.version , packet1.passkey );
-	mavlink_msg_change_operator_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_change_operator_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.control_request , packet1.version , packet1.passkey );
-	mavlink_msg_change_operator_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_change_operator_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_change_operator_control_send(MAVLINK_COMM_1 , packet1.target_system , packet1.control_request , packet1.version , packet1.passkey );
-	mavlink_msg_change_operator_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_change_operator_control_ack(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_change_operator_control_ack_t packet_in = {
-		5,
-	}72,
-	}139,
-	};
-	mavlink_change_operator_control_ack_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.gcs_system_id = packet_in.gcs_system_id;
-        	packet1.control_request = packet_in.control_request;
-        	packet1.ack = packet_in.ack;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_change_operator_control_ack_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_change_operator_control_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_change_operator_control_ack_pack(system_id, component_id, &msg , packet1.gcs_system_id , packet1.control_request , packet1.ack );
-	mavlink_msg_change_operator_control_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.gcs_system_id , packet1.control_request , packet1.ack );
-	mavlink_msg_change_operator_control_ack_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_change_operator_control_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_change_operator_control_ack_send(MAVLINK_COMM_1 , packet1.gcs_system_id , packet1.control_request , packet1.ack );
-	mavlink_msg_change_operator_control_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_auth_key(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_auth_key_t packet_in = {
-		"ABCDEFGHIJKLMNOPQRSTUVWXYZABCDE",
-	};
-	mavlink_auth_key_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.key, packet_in.key, sizeof(char)*32);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_auth_key_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_auth_key_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_auth_key_pack(system_id, component_id, &msg , packet1.key );
-	mavlink_msg_auth_key_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_auth_key_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.key );
-	mavlink_msg_auth_key_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_auth_key_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_auth_key_send(MAVLINK_COMM_1 , packet1.key );
-	mavlink_msg_auth_key_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_mode(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_set_mode_t packet_in = {
-		963497464,
-	}17,
-	}84,
-	};
-	mavlink_set_mode_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.custom_mode = packet_in.custom_mode;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.base_mode = packet_in.base_mode;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mode_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_mode_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mode_pack(system_id, component_id, &msg , packet1.target_system , packet1.base_mode , packet1.custom_mode );
-	mavlink_msg_set_mode_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mode_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.base_mode , packet1.custom_mode );
-	mavlink_msg_set_mode_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_set_mode_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mode_send(MAVLINK_COMM_1 , packet1.target_system , packet1.base_mode , packet1.custom_mode );
-	mavlink_msg_set_mode_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_param_request_read(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_param_request_read_t packet_in = {
-		17235,
-	}139,
-	}206,
-	}"EFGHIJKLMNOPQRS",
-	};
-	mavlink_param_request_read_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.param_index = packet_in.param_index;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        	mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_request_read_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_param_request_read_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_request_read_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index );
-	mavlink_msg_param_request_read_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_request_read_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index );
-	mavlink_msg_param_request_read_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_param_request_read_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_request_read_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index );
-	mavlink_msg_param_request_read_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_param_request_list(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_param_request_list_t packet_in = {
-		5,
-	}72,
-	};
-	mavlink_param_request_list_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_request_list_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_param_request_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_param_request_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_param_request_list_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_param_request_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component );
-	mavlink_msg_param_request_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_param_value(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_param_value_t packet_in = {
-		17.0,
-	}17443,
-	}17547,
-	}"IJKLMNOPQRSTUVW",
-	}77,
-	};
-	mavlink_param_value_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.param_value = packet_in.param_value;
-        	packet1.param_count = packet_in.param_count;
-        	packet1.param_index = packet_in.param_index;
-        	packet1.param_type = packet_in.param_type;
-        
-        	mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_value_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_param_value_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_value_pack(system_id, component_id, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_count , packet1.param_index );
-	mavlink_msg_param_value_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_value_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_count , packet1.param_index );
-	mavlink_msg_param_value_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_param_value_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_value_send(MAVLINK_COMM_1 , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_count , packet1.param_index );
-	mavlink_msg_param_value_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_param_set(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_param_set_t packet_in = {
-		17.0,
-	}17,
-	}84,
-	}"GHIJKLMNOPQRSTU",
-	}199,
-	};
-	mavlink_param_set_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.param_value = packet_in.param_value;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.param_type = packet_in.param_type;
-        
-        	mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_set_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_param_set_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_set_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type );
-	mavlink_msg_param_set_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type );
-	mavlink_msg_param_set_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_param_set_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_param_set_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type );
-	mavlink_msg_param_set_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_gps_raw_int(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_gps_raw_int_t packet_in = {
-		93372036854775807ULL,
-	}963497880,
-	}963498088,
-	}963498296,
-	}18275,
-	}18379,
-	}18483,
-	}18587,
-	}89,
-	}156,
-	};
-	mavlink_gps_raw_int_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.eph = packet_in.eph;
-        	packet1.epv = packet_in.epv;
-        	packet1.vel = packet_in.vel;
-        	packet1.cog = packet_in.cog;
-        	packet1.fix_type = packet_in.fix_type;
-        	packet1.satellites_visible = packet_in.satellites_visible;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_raw_int_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_gps_raw_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_raw_int_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible );
-	mavlink_msg_gps_raw_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible );
-	mavlink_msg_gps_raw_int_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_gps_raw_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible );
-	mavlink_msg_gps_raw_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_gps_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_gps_status_t packet_in = {
-		5,
-	}{ 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91 },
-	}{ 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151 },
-	}{ 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211 },
-	}{ 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 },
-	}{ 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75 },
-	};
-	mavlink_gps_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.satellites_visible = packet_in.satellites_visible;
-        
-        	mav_array_memcpy(packet1.satellite_prn, packet_in.satellite_prn, sizeof(uint8_t)*20);
-        	mav_array_memcpy(packet1.satellite_used, packet_in.satellite_used, sizeof(uint8_t)*20);
-        	mav_array_memcpy(packet1.satellite_elevation, packet_in.satellite_elevation, sizeof(uint8_t)*20);
-        	mav_array_memcpy(packet1.satellite_azimuth, packet_in.satellite_azimuth, sizeof(uint8_t)*20);
-        	mav_array_memcpy(packet1.satellite_snr, packet_in.satellite_snr, sizeof(uint8_t)*20);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_gps_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_status_pack(system_id, component_id, &msg , packet1.satellites_visible , packet1.satellite_prn , packet1.satellite_used , packet1.satellite_elevation , packet1.satellite_azimuth , packet1.satellite_snr );
-	mavlink_msg_gps_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.satellites_visible , packet1.satellite_prn , packet1.satellite_used , packet1.satellite_elevation , packet1.satellite_azimuth , packet1.satellite_snr );
-	mavlink_msg_gps_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_gps_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_status_send(MAVLINK_COMM_1 , packet1.satellites_visible , packet1.satellite_prn , packet1.satellite_used , packet1.satellite_elevation , packet1.satellite_azimuth , packet1.satellite_snr );
-	mavlink_msg_gps_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_scaled_imu(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_scaled_imu_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}18275,
-	};
-	mavlink_scaled_imu_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        	packet1.xgyro = packet_in.xgyro;
-        	packet1.ygyro = packet_in.ygyro;
-        	packet1.zgyro = packet_in.zgyro;
-        	packet1.xmag = packet_in.xmag;
-        	packet1.ymag = packet_in.ymag;
-        	packet1.zmag = packet_in.zmag;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_imu_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_scaled_imu_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_imu_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_scaled_imu_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_scaled_imu_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_scaled_imu_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_imu_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_scaled_imu_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_raw_imu(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_raw_imu_t packet_in = {
-		93372036854775807ULL,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}18275,
-	}18379,
-	}18483,
-	};
-	mavlink_raw_imu_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        	packet1.xgyro = packet_in.xgyro;
-        	packet1.ygyro = packet_in.ygyro;
-        	packet1.zgyro = packet_in.zgyro;
-        	packet1.xmag = packet_in.xmag;
-        	packet1.ymag = packet_in.ymag;
-        	packet1.zmag = packet_in.zmag;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_imu_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_raw_imu_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_imu_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_raw_imu_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_raw_imu_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_raw_imu_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_imu_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_raw_imu_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_raw_pressure(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_raw_pressure_t packet_in = {
-		93372036854775807ULL,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	};
-	mavlink_raw_pressure_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.press_abs = packet_in.press_abs;
-        	packet1.press_diff1 = packet_in.press_diff1;
-        	packet1.press_diff2 = packet_in.press_diff2;
-        	packet1.temperature = packet_in.temperature;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_pressure_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_raw_pressure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_pressure_pack(system_id, component_id, &msg , packet1.time_usec , packet1.press_abs , packet1.press_diff1 , packet1.press_diff2 , packet1.temperature );
-	mavlink_msg_raw_pressure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_pressure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.press_abs , packet1.press_diff1 , packet1.press_diff2 , packet1.temperature );
-	mavlink_msg_raw_pressure_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_raw_pressure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_pressure_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.press_abs , packet1.press_diff1 , packet1.press_diff2 , packet1.temperature );
-	mavlink_msg_raw_pressure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_scaled_pressure(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_scaled_pressure_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}17859,
-	};
-	mavlink_scaled_pressure_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.press_abs = packet_in.press_abs;
-        	packet1.press_diff = packet_in.press_diff;
-        	packet1.temperature = packet_in.temperature;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_pressure_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_scaled_pressure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_pressure_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature );
-	mavlink_msg_scaled_pressure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature );
-	mavlink_msg_scaled_pressure_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_scaled_pressure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_pressure_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature );
-	mavlink_msg_scaled_pressure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_attitude(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_attitude_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	};
-	mavlink_attitude_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.rollspeed = packet_in.rollspeed;
-        	packet1.pitchspeed = packet_in.pitchspeed;
-        	packet1.yawspeed = packet_in.yawspeed;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_attitude_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed );
-	mavlink_msg_attitude_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed );
-	mavlink_msg_attitude_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_attitude_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed );
-	mavlink_msg_attitude_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_attitude_quaternion(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_attitude_quaternion_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	};
-	mavlink_attitude_quaternion_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.q1 = packet_in.q1;
-        	packet1.q2 = packet_in.q2;
-        	packet1.q3 = packet_in.q3;
-        	packet1.q4 = packet_in.q4;
-        	packet1.rollspeed = packet_in.rollspeed;
-        	packet1.pitchspeed = packet_in.pitchspeed;
-        	packet1.yawspeed = packet_in.yawspeed;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_quaternion_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_attitude_quaternion_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_quaternion_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed );
-	mavlink_msg_attitude_quaternion_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed );
-	mavlink_msg_attitude_quaternion_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_attitude_quaternion_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed );
-	mavlink_msg_attitude_quaternion_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_local_position_ned(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_local_position_ned_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	};
-	mavlink_local_position_ned_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.vx = packet_in.vx;
-        	packet1.vy = packet_in.vy;
-        	packet1.vz = packet_in.vz;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_ned_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_local_position_ned_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_ned_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz );
-	mavlink_msg_local_position_ned_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_ned_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz );
-	mavlink_msg_local_position_ned_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_local_position_ned_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_ned_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz );
-	mavlink_msg_local_position_ned_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_global_position_int(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_global_position_int_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}963498088,
-	}963498296,
-	}18275,
-	}18379,
-	}18483,
-	}18587,
-	};
-	mavlink_global_position_int_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.relative_alt = packet_in.relative_alt;
-        	packet1.vx = packet_in.vx;
-        	packet1.vy = packet_in.vy;
-        	packet1.vz = packet_in.vz;
-        	packet1.hdg = packet_in.hdg;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_position_int_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_global_position_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_position_int_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.vx , packet1.vy , packet1.vz , packet1.hdg );
-	mavlink_msg_global_position_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_position_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.vx , packet1.vy , packet1.vz , packet1.hdg );
-	mavlink_msg_global_position_int_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_global_position_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_position_int_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.vx , packet1.vy , packet1.vz , packet1.hdg );
-	mavlink_msg_global_position_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_rc_channels_scaled(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_scaled_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}65,
-	}132,
-	};
-	mavlink_rc_channels_scaled_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.chan1_scaled = packet_in.chan1_scaled;
-        	packet1.chan2_scaled = packet_in.chan2_scaled;
-        	packet1.chan3_scaled = packet_in.chan3_scaled;
-        	packet1.chan4_scaled = packet_in.chan4_scaled;
-        	packet1.chan5_scaled = packet_in.chan5_scaled;
-        	packet1.chan6_scaled = packet_in.chan6_scaled;
-        	packet1.chan7_scaled = packet_in.chan7_scaled;
-        	packet1.chan8_scaled = packet_in.chan8_scaled;
-        	packet1.port = packet_in.port;
-        	packet1.rssi = packet_in.rssi;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_scaled_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_rc_channels_scaled_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_scaled_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.port , packet1.chan1_scaled , packet1.chan2_scaled , packet1.chan3_scaled , packet1.chan4_scaled , packet1.chan5_scaled , packet1.chan6_scaled , packet1.chan7_scaled , packet1.chan8_scaled , packet1.rssi );
-	mavlink_msg_rc_channels_scaled_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.port , packet1.chan1_scaled , packet1.chan2_scaled , packet1.chan3_scaled , packet1.chan4_scaled , packet1.chan5_scaled , packet1.chan6_scaled , packet1.chan7_scaled , packet1.chan8_scaled , packet1.rssi );
-	mavlink_msg_rc_channels_scaled_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_scaled_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_scaled_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.port , packet1.chan1_scaled , packet1.chan2_scaled , packet1.chan3_scaled , packet1.chan4_scaled , packet1.chan5_scaled , packet1.chan6_scaled , packet1.chan7_scaled , packet1.chan8_scaled , packet1.rssi );
-	mavlink_msg_rc_channels_scaled_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_rc_channels_raw(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_raw_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}65,
-	}132,
-	};
-	mavlink_rc_channels_raw_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.port = packet_in.port;
-        	packet1.rssi = packet_in.rssi;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_raw_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_rc_channels_raw_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_raw_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.port , 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.rssi );
-	mavlink_msg_rc_channels_raw_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.port , 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.rssi );
-	mavlink_msg_rc_channels_raw_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_raw_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_raw_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.port , 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.rssi );
-	mavlink_msg_rc_channels_raw_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_servo_output_raw(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_servo_output_raw_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}65,
-	};
-	mavlink_servo_output_raw_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.servo1_raw = packet_in.servo1_raw;
-        	packet1.servo2_raw = packet_in.servo2_raw;
-        	packet1.servo3_raw = packet_in.servo3_raw;
-        	packet1.servo4_raw = packet_in.servo4_raw;
-        	packet1.servo5_raw = packet_in.servo5_raw;
-        	packet1.servo6_raw = packet_in.servo6_raw;
-        	packet1.servo7_raw = packet_in.servo7_raw;
-        	packet1.servo8_raw = packet_in.servo8_raw;
-        	packet1.port = packet_in.port;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_servo_output_raw_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_servo_output_raw_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_servo_output_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw );
-	mavlink_msg_servo_output_raw_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw );
-	mavlink_msg_servo_output_raw_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_servo_output_raw_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_servo_output_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw );
-	mavlink_msg_servo_output_raw_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_request_partial_list(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_mission_request_partial_list_t packet_in = {
-		17235,
-	}17339,
-	}17,
-	}84,
-	};
-	mavlink_mission_request_partial_list_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.start_index = packet_in.start_index;
-        	packet1.end_index = packet_in.end_index;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_partial_list_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_request_partial_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index );
-	mavlink_msg_mission_request_partial_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index );
-	mavlink_msg_mission_request_partial_list_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_mission_request_partial_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index );
-	mavlink_msg_mission_request_partial_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_write_partial_list(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_mission_write_partial_list_t packet_in = {
-		17235,
-	}17339,
-	}17,
-	}84,
-	};
-	mavlink_mission_write_partial_list_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.start_index = packet_in.start_index;
-        	packet1.end_index = packet_in.end_index;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_write_partial_list_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_write_partial_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_write_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index );
-	mavlink_msg_mission_write_partial_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index );
-	mavlink_msg_mission_write_partial_list_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_mission_write_partial_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_write_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index );
-	mavlink_msg_mission_write_partial_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_item(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_mission_item_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}18691,
-	}18795,
-	}101,
-	}168,
-	}235,
-	}46,
-	}113,
-	};
-	mavlink_mission_item_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.param1 = packet_in.param1;
-        	packet1.param2 = packet_in.param2;
-        	packet1.param3 = packet_in.param3;
-        	packet1.param4 = packet_in.param4;
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.seq = packet_in.seq;
-        	packet1.command = packet_in.command;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.frame = packet_in.frame;
-        	packet1.current = packet_in.current;
-        	packet1.autocontinue = packet_in.autocontinue;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_item_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_item_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_mission_item_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_item_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_mission_item_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_mission_item_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_item_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_mission_item_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_request(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_mission_request_t packet_in = {
-		17235,
-	}139,
-	}206,
-	};
-	mavlink_mission_request_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.seq = packet_in.seq;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_request_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq );
-	mavlink_msg_mission_request_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq );
-	mavlink_msg_mission_request_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_mission_request_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq );
-	mavlink_msg_mission_request_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_set_current(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_mission_set_current_t packet_in = {
-		17235,
-	}139,
-	}206,
-	};
-	mavlink_mission_set_current_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.seq = packet_in.seq;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_set_current_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_set_current_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_set_current_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq );
-	mavlink_msg_mission_set_current_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_set_current_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq );
-	mavlink_msg_mission_set_current_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_mission_set_current_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_set_current_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq );
-	mavlink_msg_mission_set_current_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_current(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_mission_current_t packet_in = {
-		17235,
-	};
-	mavlink_mission_current_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.seq = packet_in.seq;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_current_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_current_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_current_pack(system_id, component_id, &msg , packet1.seq );
-	mavlink_msg_mission_current_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_current_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seq );
-	mavlink_msg_mission_current_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_mission_current_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_current_send(MAVLINK_COMM_1 , packet1.seq );
-	mavlink_msg_mission_current_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_request_list(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_mission_request_list_t packet_in = {
-		5,
-	}72,
-	};
-	mavlink_mission_request_list_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_list_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_request_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_mission_request_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_mission_request_list_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_mission_request_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component );
-	mavlink_msg_mission_request_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_count(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_mission_count_t packet_in = {
-		17235,
-	}139,
-	}206,
-	};
-	mavlink_mission_count_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.count = packet_in.count;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_count_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_count_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_count_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.count );
-	mavlink_msg_mission_count_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_count_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.count );
-	mavlink_msg_mission_count_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_mission_count_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_count_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.count );
-	mavlink_msg_mission_count_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_clear_all(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_mission_clear_all_t packet_in = {
-		5,
-	}72,
-	};
-	mavlink_mission_clear_all_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_clear_all_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_clear_all_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_clear_all_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_mission_clear_all_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_mission_clear_all_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_mission_clear_all_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_clear_all_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component );
-	mavlink_msg_mission_clear_all_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_item_reached(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_mission_item_reached_t packet_in = {
-		17235,
-	};
-	mavlink_mission_item_reached_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.seq = packet_in.seq;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_item_reached_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_item_reached_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_item_reached_pack(system_id, component_id, &msg , packet1.seq );
-	mavlink_msg_mission_item_reached_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seq );
-	mavlink_msg_mission_item_reached_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_mission_item_reached_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_item_reached_send(MAVLINK_COMM_1 , packet1.seq );
-	mavlink_msg_mission_item_reached_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mission_ack(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_mission_ack_t packet_in = {
-		5,
-	}72,
-	}139,
-	};
-	mavlink_mission_ack_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.type = packet_in.type;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_ack_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mission_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.type );
-	mavlink_msg_mission_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.type );
-	mavlink_msg_mission_ack_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_mission_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mission_ack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.type );
-	mavlink_msg_mission_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_gps_global_origin(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_set_gps_global_origin_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}41,
-	};
-	mavlink_set_gps_global_origin_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.latitude = packet_in.latitude;
-        	packet1.longitude = packet_in.longitude;
-        	packet1.altitude = packet_in.altitude;
-        	packet1.target_system = packet_in.target_system;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_gps_global_origin_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_gps_global_origin_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_gps_global_origin_pack(system_id, component_id, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude );
-	mavlink_msg_set_gps_global_origin_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude );
-	mavlink_msg_set_gps_global_origin_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_set_gps_global_origin_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_gps_global_origin_send(MAVLINK_COMM_1 , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude );
-	mavlink_msg_set_gps_global_origin_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_gps_global_origin(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_gps_global_origin_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	};
-	mavlink_gps_global_origin_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.latitude = packet_in.latitude;
-        	packet1.longitude = packet_in.longitude;
-        	packet1.altitude = packet_in.altitude;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_global_origin_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_gps_global_origin_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_global_origin_pack(system_id, component_id, &msg , packet1.latitude , packet1.longitude , packet1.altitude );
-	mavlink_msg_gps_global_origin_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.latitude , packet1.longitude , packet1.altitude );
-	mavlink_msg_gps_global_origin_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_gps_global_origin_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_1 , packet1.latitude , packet1.longitude , packet1.altitude );
-	mavlink_msg_gps_global_origin_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_local_position_setpoint(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_set_local_position_setpoint_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}53,
-	}120,
-	}187,
-	};
-	mavlink_set_local_position_setpoint_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.coordinate_frame = packet_in.coordinate_frame;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_local_position_setpoint_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_local_position_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_local_position_setpoint_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_set_local_position_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_local_position_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_set_local_position_setpoint_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_set_local_position_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_local_position_setpoint_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_set_local_position_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_local_position_setpoint(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_local_position_setpoint_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}53,
-	};
-	mavlink_local_position_setpoint_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.coordinate_frame = packet_in.coordinate_frame;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_setpoint_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_local_position_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_setpoint_pack(system_id, component_id, &msg , packet1.coordinate_frame , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_local_position_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.coordinate_frame , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_local_position_setpoint_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_local_position_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_1 , packet1.coordinate_frame , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_local_position_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_global_position_setpoint_int(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_global_position_setpoint_int_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}17859,
-	}175,
-	};
-	mavlink_global_position_setpoint_int_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.latitude = packet_in.latitude;
-        	packet1.longitude = packet_in.longitude;
-        	packet1.altitude = packet_in.altitude;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.coordinate_frame = packet_in.coordinate_frame;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_position_setpoint_int_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_global_position_setpoint_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_position_setpoint_int_pack(system_id, component_id, &msg , packet1.coordinate_frame , packet1.latitude , packet1.longitude , packet1.altitude , packet1.yaw );
-	mavlink_msg_global_position_setpoint_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_position_setpoint_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.coordinate_frame , packet1.latitude , packet1.longitude , packet1.altitude , packet1.yaw );
-	mavlink_msg_global_position_setpoint_int_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_global_position_setpoint_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_1 , packet1.coordinate_frame , packet1.latitude , packet1.longitude , packet1.altitude , packet1.yaw );
-	mavlink_msg_global_position_setpoint_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_global_position_setpoint_int(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_set_global_position_setpoint_int_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}17859,
-	}175,
-	};
-	mavlink_set_global_position_setpoint_int_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.latitude = packet_in.latitude;
-        	packet1.longitude = packet_in.longitude;
-        	packet1.altitude = packet_in.altitude;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.coordinate_frame = packet_in.coordinate_frame;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_global_position_setpoint_int_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_global_position_setpoint_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_global_position_setpoint_int_pack(system_id, component_id, &msg , packet1.coordinate_frame , packet1.latitude , packet1.longitude , packet1.altitude , packet1.yaw );
-	mavlink_msg_set_global_position_setpoint_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_global_position_setpoint_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.coordinate_frame , packet1.latitude , packet1.longitude , packet1.altitude , packet1.yaw );
-	mavlink_msg_set_global_position_setpoint_int_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_set_global_position_setpoint_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_global_position_setpoint_int_send(MAVLINK_COMM_1 , packet1.coordinate_frame , packet1.latitude , packet1.longitude , packet1.altitude , packet1.yaw );
-	mavlink_msg_set_global_position_setpoint_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_safety_set_allowed_area(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_safety_set_allowed_area_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}77,
-	}144,
-	}211,
-	};
-	mavlink_safety_set_allowed_area_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.p1x = packet_in.p1x;
-        	packet1.p1y = packet_in.p1y;
-        	packet1.p1z = packet_in.p1z;
-        	packet1.p2x = packet_in.p2x;
-        	packet1.p2y = packet_in.p2y;
-        	packet1.p2z = packet_in.p2z;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.frame = packet_in.frame;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_safety_set_allowed_area_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_safety_set_allowed_area_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z );
-	mavlink_msg_safety_set_allowed_area_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z );
-	mavlink_msg_safety_set_allowed_area_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_safety_set_allowed_area_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_safety_set_allowed_area_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z );
-	mavlink_msg_safety_set_allowed_area_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_safety_allowed_area(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_safety_allowed_area_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}77,
-	};
-	mavlink_safety_allowed_area_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.p1x = packet_in.p1x;
-        	packet1.p1y = packet_in.p1y;
-        	packet1.p1z = packet_in.p1z;
-        	packet1.p2x = packet_in.p2x;
-        	packet1.p2y = packet_in.p2y;
-        	packet1.p2z = packet_in.p2z;
-        	packet1.frame = packet_in.frame;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_safety_allowed_area_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_safety_allowed_area_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_safety_allowed_area_pack(system_id, component_id, &msg , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z );
-	mavlink_msg_safety_allowed_area_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z );
-	mavlink_msg_safety_allowed_area_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_safety_allowed_area_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_safety_allowed_area_send(MAVLINK_COMM_1 , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z );
-	mavlink_msg_safety_allowed_area_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_roll_pitch_yaw_thrust(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_set_roll_pitch_yaw_thrust_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}53,
-	}120,
-	};
-	mavlink_set_roll_pitch_yaw_thrust_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.thrust = packet_in.thrust;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_roll_pitch_yaw_thrust_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_roll_pitch_yaw_thrust_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_roll_pitch_yaw_thrust_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_roll_pitch_yaw_thrust_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_set_roll_pitch_yaw_thrust_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_roll_pitch_yaw_thrust_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_roll_pitch_yaw_thrust_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_roll_pitch_yaw_speed_thrust(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_set_roll_pitch_yaw_speed_thrust_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}53,
-	}120,
-	};
-	mavlink_set_roll_pitch_yaw_speed_thrust_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.roll_speed = packet_in.roll_speed;
-        	packet1.pitch_speed = packet_in.pitch_speed;
-        	packet1.yaw_speed = packet_in.yaw_speed;
-        	packet1.thrust = packet_in.thrust;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust );
-	mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust );
-	mavlink_msg_set_roll_pitch_yaw_speed_thrust_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_set_roll_pitch_yaw_speed_thrust_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust );
-	mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_roll_pitch_yaw_thrust_setpoint(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_roll_pitch_yaw_thrust_setpoint_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	};
-	mavlink_roll_pitch_yaw_thrust_setpoint_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.thrust = packet_in.thrust;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_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_roll_pitch_yaw_thrust_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_roll_pitch_yaw_speed_thrust_setpoint(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_roll_pitch_yaw_speed_thrust_setpoint_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	};
-	mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.roll_speed = packet_in.roll_speed;
-        	packet1.pitch_speed = packet_in.pitch_speed;
-        	packet1.yaw_speed = packet_in.yaw_speed;
-        	packet1.thrust = packet_in.thrust;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_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_roll_pitch_yaw_speed_thrust_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_quad_motors_setpoint(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_set_quad_motors_setpoint_t packet_in = {
-		17235,
-	}17339,
-	}17443,
-	}17547,
-	}29,
-	};
-	mavlink_set_quad_motors_setpoint_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.motor_front_nw = packet_in.motor_front_nw;
-        	packet1.motor_right_ne = packet_in.motor_right_ne;
-        	packet1.motor_back_se = packet_in.motor_back_se;
-        	packet1.motor_left_sw = packet_in.motor_left_sw;
-        	packet1.target_system = packet_in.target_system;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_motors_setpoint_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_quad_motors_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_motors_setpoint_pack(system_id, component_id, &msg , packet1.target_system , packet1.motor_front_nw , packet1.motor_right_ne , packet1.motor_back_se , packet1.motor_left_sw );
-	mavlink_msg_set_quad_motors_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_motors_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.motor_front_nw , packet1.motor_right_ne , packet1.motor_back_se , packet1.motor_left_sw );
-	mavlink_msg_set_quad_motors_setpoint_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_set_quad_motors_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_motors_setpoint_send(MAVLINK_COMM_1 , packet1.target_system , packet1.motor_front_nw , packet1.motor_right_ne , packet1.motor_back_se , packet1.motor_left_sw );
-	mavlink_msg_set_quad_motors_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_quad_swarm_roll_pitch_yaw_thrust(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_set_quad_swarm_roll_pitch_yaw_thrust_t packet_in = {
-		{ 17235, 17236, 17237, 17238 },
-	}{ 17651, 17652, 17653, 17654 },
-	}{ 18067, 18068, 18069, 18070 },
-	}{ 18483, 18484, 18485, 18486 },
-	}101,
-	}168,
-	};
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.group = packet_in.group;
-        	packet1.mode = packet_in.mode;
-        
-        	mav_array_memcpy(packet1.roll, packet_in.roll, sizeof(int16_t)*4);
-        	mav_array_memcpy(packet1.pitch, packet_in.pitch, sizeof(int16_t)*4);
-        	mav_array_memcpy(packet1.yaw, packet_in.yaw, sizeof(int16_t)*4);
-        	mav_array_memcpy(packet1.thrust, packet_in.thrust, sizeof(uint16_t)*4);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(system_id, component_id, &msg , packet1.group , packet1.mode , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.group , packet1.mode , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_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_set_quad_swarm_roll_pitch_yaw_thrust_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send(MAVLINK_COMM_1 , packet1.group , packet1.mode , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_nav_controller_output(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_nav_controller_output_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}18275,
-	}18379,
-	}18483,
-	};
-	mavlink_nav_controller_output_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.nav_roll = packet_in.nav_roll;
-        	packet1.nav_pitch = packet_in.nav_pitch;
-        	packet1.alt_error = packet_in.alt_error;
-        	packet1.aspd_error = packet_in.aspd_error;
-        	packet1.xtrack_error = packet_in.xtrack_error;
-        	packet1.nav_bearing = packet_in.nav_bearing;
-        	packet1.target_bearing = packet_in.target_bearing;
-        	packet1.wp_dist = packet_in.wp_dist;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_nav_controller_output_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_nav_controller_output_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_nav_controller_output_pack(system_id, component_id, &msg , packet1.nav_roll , packet1.nav_pitch , packet1.nav_bearing , packet1.target_bearing , packet1.wp_dist , packet1.alt_error , packet1.aspd_error , packet1.xtrack_error );
-	mavlink_msg_nav_controller_output_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.nav_roll , packet1.nav_pitch , packet1.nav_bearing , packet1.target_bearing , packet1.wp_dist , packet1.alt_error , packet1.aspd_error , packet1.xtrack_error );
-	mavlink_msg_nav_controller_output_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_nav_controller_output_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_nav_controller_output_send(MAVLINK_COMM_1 , packet1.nav_roll , packet1.nav_pitch , packet1.nav_bearing , packet1.target_bearing , packet1.wp_dist , packet1.alt_error , packet1.aspd_error , packet1.xtrack_error );
-	mavlink_msg_nav_controller_output_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_quad_swarm_led_roll_pitch_yaw_thrust(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_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet_in = {
-		{ 17235, 17236, 17237, 17238 },
-	}{ 17651, 17652, 17653, 17654 },
-	}{ 18067, 18068, 18069, 18070 },
-	}{ 18483, 18484, 18485, 18486 },
-	}101,
-	}168,
-	}{ 235, 236, 237, 238 },
-	}{ 247, 248, 249, 250 },
-	}{ 3, 4, 5, 6 },
-	};
-	mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.group = packet_in.group;
-        	packet1.mode = packet_in.mode;
-        
-        	mav_array_memcpy(packet1.roll, packet_in.roll, sizeof(int16_t)*4);
-        	mav_array_memcpy(packet1.pitch, packet_in.pitch, sizeof(int16_t)*4);
-        	mav_array_memcpy(packet1.yaw, packet_in.yaw, sizeof(int16_t)*4);
-        	mav_array_memcpy(packet1.thrust, packet_in.thrust, sizeof(uint16_t)*4);
-        	mav_array_memcpy(packet1.led_red, packet_in.led_red, sizeof(uint8_t)*4);
-        	mav_array_memcpy(packet1.led_blue, packet_in.led_blue, sizeof(uint8_t)*4);
-        	mav_array_memcpy(packet1.led_green, packet_in.led_green, sizeof(uint8_t)*4);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(system_id, component_id, &msg , packet1.group , packet1.mode , packet1.led_red , packet1.led_blue , packet1.led_green , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.group , packet1.mode , packet1.led_red , packet1.led_blue , packet1.led_green , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_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_set_quad_swarm_led_roll_pitch_yaw_thrust_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(MAVLINK_COMM_1 , packet1.group , packet1.mode , packet1.led_red , packet1.led_blue , packet1.led_green , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust );
-	mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_state_correction(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_state_correction_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	};
-	mavlink_state_correction_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.xErr = packet_in.xErr;
-        	packet1.yErr = packet_in.yErr;
-        	packet1.zErr = packet_in.zErr;
-        	packet1.rollErr = packet_in.rollErr;
-        	packet1.pitchErr = packet_in.pitchErr;
-        	packet1.yawErr = packet_in.yawErr;
-        	packet1.vxErr = packet_in.vxErr;
-        	packet1.vyErr = packet_in.vyErr;
-        	packet1.vzErr = packet_in.vzErr;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_state_correction_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_state_correction_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_state_correction_pack(system_id, component_id, &msg , packet1.xErr , packet1.yErr , packet1.zErr , packet1.rollErr , packet1.pitchErr , packet1.yawErr , packet1.vxErr , packet1.vyErr , packet1.vzErr );
-	mavlink_msg_state_correction_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_state_correction_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.xErr , packet1.yErr , packet1.zErr , packet1.rollErr , packet1.pitchErr , packet1.yawErr , packet1.vxErr , packet1.vyErr , packet1.vzErr );
-	mavlink_msg_state_correction_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_state_correction_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_state_correction_send(MAVLINK_COMM_1 , packet1.xErr , packet1.yErr , packet1.zErr , packet1.rollErr , packet1.pitchErr , packet1.yawErr , packet1.vxErr , packet1.vyErr , packet1.vzErr );
-	mavlink_msg_state_correction_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;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_request_data_stream_t packet_in = {
-		17235,
-	}139,
-	}206,
-	}17,
-	}84,
-	};
-	mavlink_request_data_stream_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.req_message_rate = packet_in.req_message_rate;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.req_stream_id = packet_in.req_stream_id;
-        	packet1.start_stop = packet_in.start_stop;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_request_data_stream_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_request_data_stream_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_request_data_stream_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.req_stream_id , packet1.req_message_rate , packet1.start_stop );
-	mavlink_msg_request_data_stream_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_request_data_stream_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.req_stream_id , packet1.req_message_rate , packet1.start_stop );
-	mavlink_msg_request_data_stream_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_request_data_stream_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_request_data_stream_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.req_stream_id , packet1.req_message_rate , packet1.start_stop );
-	mavlink_msg_request_data_stream_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data_stream(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_data_stream_t packet_in = {
-		17235,
-	}139,
-	}206,
-	};
-	mavlink_data_stream_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.message_rate = packet_in.message_rate;
-        	packet1.stream_id = packet_in.stream_id;
-        	packet1.on_off = packet_in.on_off;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_stream_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data_stream_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_stream_pack(system_id, component_id, &msg , packet1.stream_id , packet1.message_rate , packet1.on_off );
-	mavlink_msg_data_stream_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_stream_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.stream_id , packet1.message_rate , packet1.on_off );
-	mavlink_msg_data_stream_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_data_stream_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_stream_send(MAVLINK_COMM_1 , packet1.stream_id , packet1.message_rate , packet1.on_off );
-	mavlink_msg_data_stream_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_manual_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_manual_control_t packet_in = {
-		17235,
-	}17339,
-	}17443,
-	}17547,
-	}17651,
-	}163,
-	};
-	mavlink_manual_control_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.r = packet_in.r;
-        	packet1.buttons = packet_in.buttons;
-        	packet1.target = packet_in.target;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_manual_control_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_manual_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_manual_control_pack(system_id, component_id, &msg , packet1.target , packet1.x , packet1.y , packet1.z , packet1.r , packet1.buttons );
-	mavlink_msg_manual_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_manual_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target , packet1.x , packet1.y , packet1.z , packet1.r , packet1.buttons );
-	mavlink_msg_manual_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_manual_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_manual_control_send(MAVLINK_COMM_1 , packet1.target , packet1.x , packet1.y , packet1.z , packet1.r , packet1.buttons );
-	mavlink_msg_manual_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_rc_channels_override(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_override_t packet_in = {
-		17235,
-	}17339,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}53,
-	}120,
-	};
-	mavlink_rc_channels_override_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	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.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_override_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_rc_channels_override_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_override_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw );
-	mavlink_msg_rc_channels_override_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw );
-	mavlink_msg_rc_channels_override_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_override_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rc_channels_override_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw );
-	mavlink_msg_rc_channels_override_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_vfr_hud(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_vfr_hud_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}18067,
-	}18171,
-	};
-	mavlink_vfr_hud_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.airspeed = packet_in.airspeed;
-        	packet1.groundspeed = packet_in.groundspeed;
-        	packet1.alt = packet_in.alt;
-        	packet1.climb = packet_in.climb;
-        	packet1.heading = packet_in.heading;
-        	packet1.throttle = packet_in.throttle;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vfr_hud_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_vfr_hud_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vfr_hud_pack(system_id, component_id, &msg , packet1.airspeed , packet1.groundspeed , packet1.heading , packet1.throttle , packet1.alt , packet1.climb );
-	mavlink_msg_vfr_hud_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vfr_hud_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.airspeed , packet1.groundspeed , packet1.heading , packet1.throttle , packet1.alt , packet1.climb );
-	mavlink_msg_vfr_hud_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_vfr_hud_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_1 , packet1.airspeed , packet1.groundspeed , packet1.heading , packet1.throttle , packet1.alt , packet1.climb );
-	mavlink_msg_vfr_hud_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_command_long(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_command_long_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}18691,
-	}223,
-	}34,
-	}101,
-	};
-	mavlink_command_long_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.param1 = packet_in.param1;
-        	packet1.param2 = packet_in.param2;
-        	packet1.param3 = packet_in.param3;
-        	packet1.param4 = packet_in.param4;
-        	packet1.param5 = packet_in.param5;
-        	packet1.param6 = packet_in.param6;
-        	packet1.param7 = packet_in.param7;
-        	packet1.command = packet_in.command;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.confirmation = packet_in.confirmation;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_command_long_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_command_long_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_command_long_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.command , packet1.confirmation , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.param5 , packet1.param6 , packet1.param7 );
-	mavlink_msg_command_long_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_command_long_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.command , packet1.confirmation , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.param5 , packet1.param6 , packet1.param7 );
-	mavlink_msg_command_long_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_command_long_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_command_long_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.command , packet1.confirmation , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.param5 , packet1.param6 , packet1.param7 );
-	mavlink_msg_command_long_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_command_ack(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_command_ack_t packet_in = {
-		17235,
-	}139,
-	};
-	mavlink_command_ack_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.command = packet_in.command;
-        	packet1.result = packet_in.result;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_command_ack_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_command_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_command_ack_pack(system_id, component_id, &msg , packet1.command , packet1.result );
-	mavlink_msg_command_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_command_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command , packet1.result );
-	mavlink_msg_command_ack_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_command_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_command_ack_send(MAVLINK_COMM_1 , packet1.command , packet1.result );
-	mavlink_msg_command_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_roll_pitch_yaw_rates_thrust_setpoint(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_roll_pitch_yaw_rates_thrust_setpoint_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	};
-	mavlink_roll_pitch_yaw_rates_thrust_setpoint_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.roll_rate = packet_in.roll_rate;
-        	packet1.pitch_rate = packet_in.pitch_rate;
-        	packet1.yaw_rate = packet_in.yaw_rate;
-        	packet1.thrust = packet_in.thrust;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.roll_rate , packet1.pitch_rate , packet1.yaw_rate , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.roll_rate , packet1.pitch_rate , packet1.yaw_rate , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_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_roll_pitch_yaw_rates_thrust_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.roll_rate , packet1.pitch_rate , packet1.yaw_rate , packet1.thrust );
-	mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_manual_setpoint(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_manual_setpoint_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}65,
-	}132,
-	};
-	mavlink_manual_setpoint_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.thrust = packet_in.thrust;
-        	packet1.mode_switch = packet_in.mode_switch;
-        	packet1.manual_override_switch = packet_in.manual_override_switch;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_manual_setpoint_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_manual_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_manual_setpoint_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.mode_switch , packet1.manual_override_switch );
-	mavlink_msg_manual_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_manual_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.mode_switch , packet1.manual_override_switch );
-	mavlink_msg_manual_setpoint_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_manual_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_manual_setpoint_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.mode_switch , packet1.manual_override_switch );
-	mavlink_msg_manual_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_local_position_ned_system_global_offset(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_local_position_ned_system_global_offset_t packet_in = {
-		963497464,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	};
-	mavlink_local_position_ned_system_global_offset_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_ned_system_global_offset_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_local_position_ned_system_global_offset_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_ned_system_global_offset_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_local_position_ned_system_global_offset_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_local_position_ned_system_global_offset_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_local_position_ned_system_global_offset_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_local_position_ned_system_global_offset_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_local_position_ned_system_global_offset_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hil_state(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_hil_state_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}963499128,
-	}963499336,
-	}963499544,
-	}19523,
-	}19627,
-	}19731,
-	}19835,
-	}19939,
-	}20043,
-	};
-	mavlink_hil_state_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.rollspeed = packet_in.rollspeed;
-        	packet1.pitchspeed = packet_in.pitchspeed;
-        	packet1.yawspeed = packet_in.yawspeed;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.vx = packet_in.vx;
-        	packet1.vy = packet_in.vy;
-        	packet1.vz = packet_in.vz;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_state_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hil_state_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_state_pack(system_id, component_id, &msg , packet1.time_usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc );
-	mavlink_msg_hil_state_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_state_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc );
-	mavlink_msg_hil_state_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_hil_state_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_state_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc );
-	mavlink_msg_hil_state_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hil_controls(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_hil_controls_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}269.0,
-	}125,
-	}192,
-	};
-	mavlink_hil_controls_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.roll_ailerons = packet_in.roll_ailerons;
-        	packet1.pitch_elevator = packet_in.pitch_elevator;
-        	packet1.yaw_rudder = packet_in.yaw_rudder;
-        	packet1.throttle = packet_in.throttle;
-        	packet1.aux1 = packet_in.aux1;
-        	packet1.aux2 = packet_in.aux2;
-        	packet1.aux3 = packet_in.aux3;
-        	packet1.aux4 = packet_in.aux4;
-        	packet1.mode = packet_in.mode;
-        	packet1.nav_mode = packet_in.nav_mode;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_controls_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hil_controls_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_controls_pack(system_id, component_id, &msg , packet1.time_usec , packet1.roll_ailerons , packet1.pitch_elevator , packet1.yaw_rudder , packet1.throttle , packet1.aux1 , packet1.aux2 , packet1.aux3 , packet1.aux4 , packet1.mode , packet1.nav_mode );
-	mavlink_msg_hil_controls_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_controls_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.roll_ailerons , packet1.pitch_elevator , packet1.yaw_rudder , packet1.throttle , packet1.aux1 , packet1.aux2 , packet1.aux3 , packet1.aux4 , packet1.mode , packet1.nav_mode );
-	mavlink_msg_hil_controls_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_hil_controls_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_controls_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.roll_ailerons , packet1.pitch_elevator , packet1.yaw_rudder , packet1.throttle , packet1.aux1 , packet1.aux2 , packet1.aux3 , packet1.aux4 , packet1.mode , packet1.nav_mode );
-	mavlink_msg_hil_controls_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hil_rc_inputs_raw(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_hil_rc_inputs_raw_t packet_in = {
-		93372036854775807ULL,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}18275,
-	}18379,
-	}18483,
-	}18587,
-	}18691,
-	}18795,
-	}101,
-	};
-	mavlink_hil_rc_inputs_raw_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	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.rssi = packet_in.rssi;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_rc_inputs_raw_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hil_rc_inputs_raw_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_rc_inputs_raw_pack(system_id, component_id, &msg , packet1.time_usec , 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.rssi );
-	mavlink_msg_hil_rc_inputs_raw_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_rc_inputs_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , 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.rssi );
-	mavlink_msg_hil_rc_inputs_raw_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_hil_rc_inputs_raw_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_rc_inputs_raw_send(MAVLINK_COMM_1 , packet1.time_usec , 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.rssi );
-	mavlink_msg_hil_rc_inputs_raw_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_optical_flow(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_optical_flow_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}18275,
-	}18379,
-	}77,
-	}144,
-	};
-	mavlink_optical_flow_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.flow_comp_m_x = packet_in.flow_comp_m_x;
-        	packet1.flow_comp_m_y = packet_in.flow_comp_m_y;
-        	packet1.ground_distance = packet_in.ground_distance;
-        	packet1.flow_x = packet_in.flow_x;
-        	packet1.flow_y = packet_in.flow_y;
-        	packet1.sensor_id = packet_in.sensor_id;
-        	packet1.quality = packet_in.quality;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_optical_flow_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_optical_flow_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_optical_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance );
-	mavlink_msg_optical_flow_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance );
-	mavlink_msg_optical_flow_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_optical_flow_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance );
-	mavlink_msg_optical_flow_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_global_vision_position_estimate(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_global_vision_position_estimate_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	};
-	mavlink_global_vision_position_estimate_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.usec = packet_in.usec;
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_vision_position_estimate_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_global_vision_position_estimate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_global_vision_position_estimate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_global_vision_position_estimate_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_global_vision_position_estimate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_global_vision_position_estimate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_vision_position_estimate(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_vision_position_estimate_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	};
-	mavlink_vision_position_estimate_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.usec = packet_in.usec;
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vision_position_estimate_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_vision_position_estimate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_vision_position_estimate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_vision_position_estimate_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_vision_position_estimate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_vision_position_estimate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_vision_speed_estimate(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_vision_speed_estimate_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	};
-	mavlink_vision_speed_estimate_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.usec = packet_in.usec;
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vision_speed_estimate_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_vision_speed_estimate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vision_speed_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_vision_speed_estimate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_vision_speed_estimate_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_vision_speed_estimate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vision_speed_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_vision_speed_estimate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_vicon_position_estimate(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_vicon_position_estimate_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	};
-	mavlink_vicon_position_estimate_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.usec = packet_in.usec;
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vicon_position_estimate_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_vicon_position_estimate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vicon_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_vicon_position_estimate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_vicon_position_estimate_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_vicon_position_estimate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_vicon_position_estimate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_highres_imu(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_highres_imu_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}269.0,
-	}297.0,
-	}325.0,
-	}353.0,
-	}381.0,
-	}409.0,
-	}20355,
-	};
-	mavlink_highres_imu_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        	packet1.xgyro = packet_in.xgyro;
-        	packet1.ygyro = packet_in.ygyro;
-        	packet1.zgyro = packet_in.zgyro;
-        	packet1.xmag = packet_in.xmag;
-        	packet1.ymag = packet_in.ymag;
-        	packet1.zmag = packet_in.zmag;
-        	packet1.abs_pressure = packet_in.abs_pressure;
-        	packet1.diff_pressure = packet_in.diff_pressure;
-        	packet1.pressure_alt = packet_in.pressure_alt;
-        	packet1.temperature = packet_in.temperature;
-        	packet1.fields_updated = packet_in.fields_updated;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_highres_imu_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_highres_imu_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_highres_imu_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated );
-	mavlink_msg_highres_imu_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_highres_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated );
-	mavlink_msg_highres_imu_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_highres_imu_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_highres_imu_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated );
-	mavlink_msg_highres_imu_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_omnidirectional_flow(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_omnidirectional_flow_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}{ 17859, 17860, 17861, 17862, 17863, 17864, 17865, 17866, 17867, 17868 },
-	}{ 18899, 18900, 18901, 18902, 18903, 18904, 18905, 18906, 18907, 18908 },
-	}161,
-	}228,
-	};
-	mavlink_omnidirectional_flow_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.front_distance_m = packet_in.front_distance_m;
-        	packet1.sensor_id = packet_in.sensor_id;
-        	packet1.quality = packet_in.quality;
-        
-        	mav_array_memcpy(packet1.left, packet_in.left, sizeof(int16_t)*10);
-        	mav_array_memcpy(packet1.right, packet_in.right, sizeof(int16_t)*10);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_omnidirectional_flow_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_omnidirectional_flow_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_omnidirectional_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.left , packet1.right , packet1.quality , packet1.front_distance_m );
-	mavlink_msg_omnidirectional_flow_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_omnidirectional_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.left , packet1.right , packet1.quality , packet1.front_distance_m );
-	mavlink_msg_omnidirectional_flow_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_omnidirectional_flow_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_omnidirectional_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.left , packet1.right , packet1.quality , packet1.front_distance_m );
-	mavlink_msg_omnidirectional_flow_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hil_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_hil_sensor_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}269.0,
-	}297.0,
-	}325.0,
-	}353.0,
-	}381.0,
-	}409.0,
-	}963500584,
-	};
-	mavlink_hil_sensor_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        	packet1.xgyro = packet_in.xgyro;
-        	packet1.ygyro = packet_in.ygyro;
-        	packet1.zgyro = packet_in.zgyro;
-        	packet1.xmag = packet_in.xmag;
-        	packet1.ymag = packet_in.ymag;
-        	packet1.zmag = packet_in.zmag;
-        	packet1.abs_pressure = packet_in.abs_pressure;
-        	packet1.diff_pressure = packet_in.diff_pressure;
-        	packet1.pressure_alt = packet_in.pressure_alt;
-        	packet1.temperature = packet_in.temperature;
-        	packet1.fields_updated = packet_in.fields_updated;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_sensor_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hil_sensor_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_sensor_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated );
-	mavlink_msg_hil_sensor_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated );
-	mavlink_msg_hil_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_hil_sensor_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_sensor_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated );
-	mavlink_msg_hil_sensor_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_sim_state(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_sim_state_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}269.0,
-	}297.0,
-	}325.0,
-	}353.0,
-	}381.0,
-	}409.0,
-	}437.0,
-	}465.0,
-	}493.0,
-	}521.0,
-	}549.0,
-	}577.0,
-	};
-	mavlink_sim_state_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.q1 = packet_in.q1;
-        	packet1.q2 = packet_in.q2;
-        	packet1.q3 = packet_in.q3;
-        	packet1.q4 = packet_in.q4;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        	packet1.xgyro = packet_in.xgyro;
-        	packet1.ygyro = packet_in.ygyro;
-        	packet1.zgyro = packet_in.zgyro;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.std_dev_horz = packet_in.std_dev_horz;
-        	packet1.std_dev_vert = packet_in.std_dev_vert;
-        	packet1.vn = packet_in.vn;
-        	packet1.ve = packet_in.ve;
-        	packet1.vd = packet_in.vd;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sim_state_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_sim_state_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sim_state_pack(system_id, component_id, &msg , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd );
-	mavlink_msg_sim_state_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sim_state_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd );
-	mavlink_msg_sim_state_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_sim_state_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sim_state_send(MAVLINK_COMM_1 , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd );
-	mavlink_msg_sim_state_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_radio_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_radio_status_t packet_in = {
-		17235,
-	}17339,
-	}17,
-	}84,
-	}151,
-	}218,
-	}29,
-	};
-	mavlink_radio_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.rxerrors = packet_in.rxerrors;
-        	packet1.fixed = packet_in.fixed;
-        	packet1.rssi = packet_in.rssi;
-        	packet1.remrssi = packet_in.remrssi;
-        	packet1.txbuf = packet_in.txbuf;
-        	packet1.noise = packet_in.noise;
-        	packet1.remnoise = packet_in.remnoise;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_radio_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_status_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_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_radio_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_status_send(MAVLINK_COMM_1 , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_file_transfer_start(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_file_transfer_start_t packet_in = {
-		93372036854775807ULL,
-	}963497880,
-	}"MNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",
-	}249,
-	}60,
-	};
-	mavlink_file_transfer_start_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.transfer_uid = packet_in.transfer_uid;
-        	packet1.file_size = packet_in.file_size;
-        	packet1.direction = packet_in.direction;
-        	packet1.flags = packet_in.flags;
-        
-        	mav_array_memcpy(packet1.dest_path, packet_in.dest_path, sizeof(char)*240);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_start_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_file_transfer_start_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_start_pack(system_id, component_id, &msg , packet1.transfer_uid , packet1.dest_path , packet1.direction , packet1.file_size , packet1.flags );
-	mavlink_msg_file_transfer_start_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_start_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.transfer_uid , packet1.dest_path , packet1.direction , packet1.file_size , packet1.flags );
-	mavlink_msg_file_transfer_start_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_file_transfer_start_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_start_send(MAVLINK_COMM_1 , packet1.transfer_uid , packet1.dest_path , packet1.direction , packet1.file_size , packet1.flags );
-	mavlink_msg_file_transfer_start_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_file_transfer_dir_list(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_file_transfer_dir_list_t packet_in = {
-		93372036854775807ULL,
-	}"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLM",
-	}237,
-	};
-	mavlink_file_transfer_dir_list_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.transfer_uid = packet_in.transfer_uid;
-        	packet1.flags = packet_in.flags;
-        
-        	mav_array_memcpy(packet1.dir_path, packet_in.dir_path, sizeof(char)*240);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_dir_list_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_file_transfer_dir_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_dir_list_pack(system_id, component_id, &msg , packet1.transfer_uid , packet1.dir_path , packet1.flags );
-	mavlink_msg_file_transfer_dir_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_dir_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.transfer_uid , packet1.dir_path , packet1.flags );
-	mavlink_msg_file_transfer_dir_list_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_file_transfer_dir_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_dir_list_send(MAVLINK_COMM_1 , packet1.transfer_uid , packet1.dir_path , packet1.flags );
-	mavlink_msg_file_transfer_dir_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_file_transfer_res(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_file_transfer_res_t packet_in = {
-		93372036854775807ULL,
-	}29,
-	};
-	mavlink_file_transfer_res_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.transfer_uid = packet_in.transfer_uid;
-        	packet1.result = packet_in.result;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_res_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_file_transfer_res_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_res_pack(system_id, component_id, &msg , packet1.transfer_uid , packet1.result );
-	mavlink_msg_file_transfer_res_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_res_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.transfer_uid , packet1.result );
-	mavlink_msg_file_transfer_res_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_file_transfer_res_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_file_transfer_res_send(MAVLINK_COMM_1 , packet1.transfer_uid , packet1.result );
-	mavlink_msg_file_transfer_res_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hil_gps(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_hil_gps_t packet_in = {
-		93372036854775807ULL,
-	}963497880,
-	}963498088,
-	}963498296,
-	}18275,
-	}18379,
-	}18483,
-	}18587,
-	}18691,
-	}18795,
-	}18899,
-	}235,
-	}46,
-	};
-	mavlink_hil_gps_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.eph = packet_in.eph;
-        	packet1.epv = packet_in.epv;
-        	packet1.vel = packet_in.vel;
-        	packet1.vn = packet_in.vn;
-        	packet1.ve = packet_in.ve;
-        	packet1.vd = packet_in.vd;
-        	packet1.cog = packet_in.cog;
-        	packet1.fix_type = packet_in.fix_type;
-        	packet1.satellites_visible = packet_in.satellites_visible;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_gps_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hil_gps_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_gps_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible );
-	mavlink_msg_hil_gps_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_gps_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible );
-	mavlink_msg_hil_gps_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_hil_gps_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_gps_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible );
-	mavlink_msg_hil_gps_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hil_optical_flow(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_hil_optical_flow_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}18275,
-	}18379,
-	}77,
-	}144,
-	};
-	mavlink_hil_optical_flow_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.flow_comp_m_x = packet_in.flow_comp_m_x;
-        	packet1.flow_comp_m_y = packet_in.flow_comp_m_y;
-        	packet1.ground_distance = packet_in.ground_distance;
-        	packet1.flow_x = packet_in.flow_x;
-        	packet1.flow_y = packet_in.flow_y;
-        	packet1.sensor_id = packet_in.sensor_id;
-        	packet1.quality = packet_in.quality;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_optical_flow_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hil_optical_flow_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_optical_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance );
-	mavlink_msg_hil_optical_flow_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance );
-	mavlink_msg_hil_optical_flow_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_hil_optical_flow_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance );
-	mavlink_msg_hil_optical_flow_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hil_state_quaternion(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_hil_state_quaternion_t packet_in = {
-		93372036854775807ULL,
-	}{ 73.0, 74.0, 75.0, 76.0 },
-	}185.0,
-	}213.0,
-	}241.0,
-	}963499336,
-	}963499544,
-	}963499752,
-	}19731,
-	}19835,
-	}19939,
-	}20043,
-	}20147,
-	}20251,
-	}20355,
-	}20459,
-	};
-	mavlink_hil_state_quaternion_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.rollspeed = packet_in.rollspeed;
-        	packet1.pitchspeed = packet_in.pitchspeed;
-        	packet1.yawspeed = packet_in.yawspeed;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.vx = packet_in.vx;
-        	packet1.vy = packet_in.vy;
-        	packet1.vz = packet_in.vz;
-        	packet1.ind_airspeed = packet_in.ind_airspeed;
-        	packet1.true_airspeed = packet_in.true_airspeed;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        
-        	mav_array_memcpy(packet1.attitude_quaternion, packet_in.attitude_quaternion, sizeof(float)*4);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_state_quaternion_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hil_state_quaternion_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_state_quaternion_pack(system_id, component_id, &msg , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
-	mavlink_msg_hil_state_quaternion_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
-	mavlink_msg_hil_state_quaternion_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_hil_state_quaternion_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hil_state_quaternion_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
-	mavlink_msg_hil_state_quaternion_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_scaled_imu2(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_scaled_imu2_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}18275,
-	};
-	mavlink_scaled_imu2_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        	packet1.xgyro = packet_in.xgyro;
-        	packet1.ygyro = packet_in.ygyro;
-        	packet1.zgyro = packet_in.zgyro;
-        	packet1.xmag = packet_in.xmag;
-        	packet1.ymag = packet_in.ymag;
-        	packet1.zmag = packet_in.zmag;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_imu2_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_scaled_imu2_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_imu2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_scaled_imu2_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_scaled_imu2_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_scaled_imu2_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_scaled_imu2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag );
-	mavlink_msg_scaled_imu2_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_log_request_list(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_log_request_list_t packet_in = {
-		17235,
-	}17339,
-	}17,
-	}84,
-	};
-	mavlink_log_request_list_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.start = packet_in.start;
-        	packet1.end = packet_in.end;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_list_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_log_request_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start , packet1.end );
-	mavlink_msg_log_request_list_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start , packet1.end );
-	mavlink_msg_log_request_list_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_log_request_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start , packet1.end );
-	mavlink_msg_log_request_list_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_log_entry(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_log_entry_t packet_in = {
-		963497464,
-	}963497672,
-	}17651,
-	}17755,
-	}17859,
-	};
-	mavlink_log_entry_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_utc = packet_in.time_utc;
-        	packet1.size = packet_in.size;
-        	packet1.id = packet_in.id;
-        	packet1.num_logs = packet_in.num_logs;
-        	packet1.last_log_num = packet_in.last_log_num;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_entry_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_log_entry_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_entry_pack(system_id, component_id, &msg , packet1.id , packet1.num_logs , packet1.last_log_num , packet1.time_utc , packet1.size );
-	mavlink_msg_log_entry_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_entry_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.num_logs , packet1.last_log_num , packet1.time_utc , packet1.size );
-	mavlink_msg_log_entry_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_log_entry_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_entry_send(MAVLINK_COMM_1 , packet1.id , packet1.num_logs , packet1.last_log_num , packet1.time_utc , packet1.size );
-	mavlink_msg_log_entry_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_log_request_data(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_log_request_data_t packet_in = {
-		963497464,
-	}963497672,
-	}17651,
-	}163,
-	}230,
-	};
-	mavlink_log_request_data_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.ofs = packet_in.ofs;
-        	packet1.count = packet_in.count;
-        	packet1.id = packet_in.id;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_data_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_log_request_data_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_data_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id , packet1.ofs , packet1.count );
-	mavlink_msg_log_request_data_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id , packet1.ofs , packet1.count );
-	mavlink_msg_log_request_data_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_log_request_data_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_data_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id , packet1.ofs , packet1.count );
-	mavlink_msg_log_request_data_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_log_data(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_log_data_t packet_in = {
-		963497464,
-	}17443,
-	}151,
-	}{ 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51 },
-	};
-	mavlink_log_data_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.ofs = packet_in.ofs;
-        	packet1.id = packet_in.id;
-        	packet1.count = packet_in.count;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*90);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_data_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_log_data_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_data_pack(system_id, component_id, &msg , packet1.id , packet1.ofs , packet1.count , packet1.data );
-	mavlink_msg_log_data_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.ofs , packet1.count , packet1.data );
-	mavlink_msg_log_data_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_log_data_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_data_send(MAVLINK_COMM_1 , packet1.id , packet1.ofs , packet1.count , packet1.data );
-	mavlink_msg_log_data_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_log_erase(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_log_erase_t packet_in = {
-		5,
-	}72,
-	};
-	mavlink_log_erase_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_erase_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_log_erase_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_erase_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_log_erase_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_erase_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_log_erase_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_log_erase_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_erase_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component );
-	mavlink_msg_log_erase_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_log_request_end(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_log_request_end_t packet_in = {
-		5,
-	}72,
-	};
-	mavlink_log_request_end_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_end_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_log_request_end_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_end_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_log_request_end_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_end_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_log_request_end_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_log_request_end_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_log_request_end_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component );
-	mavlink_msg_log_request_end_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_gps_inject_data(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_gps_inject_data_t packet_in = {
-		5,
-	}72,
-	}139,
-	}{ 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59 },
-	};
-	mavlink_gps_inject_data_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*110);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_inject_data_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_gps_inject_data_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_inject_data_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
-	mavlink_msg_gps_inject_data_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
-	mavlink_msg_gps_inject_data_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_gps_inject_data_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps_inject_data_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
-	mavlink_msg_gps_inject_data_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_gps2_raw(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_gps2_raw_t packet_in = {
-		93372036854775807ULL,
-	}963497880,
-	}963498088,
-	}963498296,
-	}963498504,
-	}18483,
-	}18587,
-	}18691,
-	}18795,
-	}101,
-	}168,
-	}235,
-	};
-	mavlink_gps2_raw_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.dgps_age = packet_in.dgps_age;
-        	packet1.eph = packet_in.eph;
-        	packet1.epv = packet_in.epv;
-        	packet1.vel = packet_in.vel;
-        	packet1.cog = packet_in.cog;
-        	packet1.fix_type = packet_in.fix_type;
-        	packet1.satellites_visible = packet_in.satellites_visible;
-        	packet1.dgps_numch = packet_in.dgps_numch;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps2_raw_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_gps2_raw_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
-	mavlink_msg_gps2_raw_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
-	mavlink_msg_gps2_raw_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_gps2_raw_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
-	mavlink_msg_gps2_raw_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;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_data_transmission_handshake_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}163,
-	}230,
-	}41,
-	};
-	mavlink_data_transmission_handshake_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.size = packet_in.size;
-        	packet1.width = packet_in.width;
-        	packet1.height = packet_in.height;
-        	packet1.packets = packet_in.packets;
-        	packet1.type = packet_in.type;
-        	packet1.payload = packet_in.payload;
-        	packet1.jpg_quality = packet_in.jpg_quality;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_transmission_handshake_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_transmission_handshake_pack(system_id, component_id, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
-	mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
-	mavlink_msg_data_transmission_handshake_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_data_transmission_handshake_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_transmission_handshake_send(MAVLINK_COMM_1 , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
-	mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_encapsulated_data(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_encapsulated_data_t packet_in = {
-		17235,
-	}{ 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, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 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 },
-	};
-	mavlink_encapsulated_data_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.seqnr = packet_in.seqnr;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*253);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_encapsulated_data_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_encapsulated_data_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_encapsulated_data_pack(system_id, component_id, &msg , packet1.seqnr , packet1.data );
-	mavlink_msg_encapsulated_data_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seqnr , packet1.data );
-	mavlink_msg_encapsulated_data_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_encapsulated_data_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_encapsulated_data_send(MAVLINK_COMM_1 , packet1.seqnr , packet1.data );
-	mavlink_msg_encapsulated_data_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;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_battery_status_t packet_in = {
-		963497464,
-	}963497672,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}18275,
-	}199,
-	}10,
-	};
-	mavlink_battery_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.current_consumed = packet_in.current_consumed;
-        	packet1.energy_consumed = packet_in.energy_consumed;
-        	packet1.voltage_cell_1 = packet_in.voltage_cell_1;
-        	packet1.voltage_cell_2 = packet_in.voltage_cell_2;
-        	packet1.voltage_cell_3 = packet_in.voltage_cell_3;
-        	packet1.voltage_cell_4 = packet_in.voltage_cell_4;
-        	packet1.voltage_cell_5 = packet_in.voltage_cell_5;
-        	packet1.voltage_cell_6 = packet_in.voltage_cell_6;
-        	packet1.current_battery = packet_in.current_battery;
-        	packet1.accu_id = packet_in.accu_id;
-        	packet1.battery_remaining = packet_in.battery_remaining;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_battery_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_battery_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_battery_status_pack(system_id, component_id, &msg , packet1.accu_id , packet1.voltage_cell_1 , packet1.voltage_cell_2 , packet1.voltage_cell_3 , packet1.voltage_cell_4 , packet1.voltage_cell_5 , packet1.voltage_cell_6 , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining );
-	mavlink_msg_battery_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_battery_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.accu_id , packet1.voltage_cell_1 , packet1.voltage_cell_2 , packet1.voltage_cell_3 , packet1.voltage_cell_4 , packet1.voltage_cell_5 , packet1.voltage_cell_6 , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining );
-	mavlink_msg_battery_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_battery_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_battery_status_send(MAVLINK_COMM_1 , packet1.accu_id , packet1.voltage_cell_1 , packet1.voltage_cell_2 , packet1.voltage_cell_3 , packet1.voltage_cell_4 , packet1.voltage_cell_5 , packet1.voltage_cell_6 , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining );
-	mavlink_msg_battery_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_setpoint_8dof(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_setpoint_8dof_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}101,
-	};
-	mavlink_setpoint_8dof_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.val1 = packet_in.val1;
-        	packet1.val2 = packet_in.val2;
-        	packet1.val3 = packet_in.val3;
-        	packet1.val4 = packet_in.val4;
-        	packet1.val5 = packet_in.val5;
-        	packet1.val6 = packet_in.val6;
-        	packet1.val7 = packet_in.val7;
-        	packet1.val8 = packet_in.val8;
-        	packet1.target_system = packet_in.target_system;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_setpoint_8dof_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_setpoint_8dof_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_setpoint_8dof_pack(system_id, component_id, &msg , packet1.target_system , packet1.val1 , packet1.val2 , packet1.val3 , packet1.val4 , packet1.val5 , packet1.val6 , packet1.val7 , packet1.val8 );
-	mavlink_msg_setpoint_8dof_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_setpoint_8dof_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.val1 , packet1.val2 , packet1.val3 , packet1.val4 , packet1.val5 , packet1.val6 , packet1.val7 , packet1.val8 );
-	mavlink_msg_setpoint_8dof_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_setpoint_8dof_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_setpoint_8dof_send(MAVLINK_COMM_1 , packet1.target_system , packet1.val1 , packet1.val2 , packet1.val3 , packet1.val4 , packet1.val5 , packet1.val6 , packet1.val7 , packet1.val8 );
-	mavlink_msg_setpoint_8dof_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_setpoint_6dof(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_setpoint_6dof_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}77,
-	};
-	mavlink_setpoint_6dof_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.trans_x = packet_in.trans_x;
-        	packet1.trans_y = packet_in.trans_y;
-        	packet1.trans_z = packet_in.trans_z;
-        	packet1.rot_x = packet_in.rot_x;
-        	packet1.rot_y = packet_in.rot_y;
-        	packet1.rot_z = packet_in.rot_z;
-        	packet1.target_system = packet_in.target_system;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_setpoint_6dof_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_setpoint_6dof_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_setpoint_6dof_pack(system_id, component_id, &msg , packet1.target_system , packet1.trans_x , packet1.trans_y , packet1.trans_z , packet1.rot_x , packet1.rot_y , packet1.rot_z );
-	mavlink_msg_setpoint_6dof_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_setpoint_6dof_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.trans_x , packet1.trans_y , packet1.trans_z , packet1.rot_x , packet1.rot_y , packet1.rot_z );
-	mavlink_msg_setpoint_6dof_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_setpoint_6dof_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_setpoint_6dof_send(MAVLINK_COMM_1 , packet1.target_system , packet1.trans_x , packet1.trans_y , packet1.trans_z , packet1.rot_x , packet1.rot_y , packet1.rot_z );
-	mavlink_msg_setpoint_6dof_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_memory_vect(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_memory_vect_t packet_in = {
-		17235,
-	}139,
-	}206,
-	}{ 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48 },
-	};
-	mavlink_memory_vect_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.address = packet_in.address;
-        	packet1.ver = packet_in.ver;
-        	packet1.type = packet_in.type;
-        
-        	mav_array_memcpy(packet1.value, packet_in.value, sizeof(int8_t)*32);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_memory_vect_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_memory_vect_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_memory_vect_pack(system_id, component_id, &msg , packet1.address , packet1.ver , packet1.type , packet1.value );
-	mavlink_msg_memory_vect_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_memory_vect_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.address , packet1.ver , packet1.type , packet1.value );
-	mavlink_msg_memory_vect_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_memory_vect_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_memory_vect_send(MAVLINK_COMM_1 , packet1.address , packet1.ver , packet1.type , packet1.value );
-	mavlink_msg_memory_vect_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_debug_vect(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_debug_vect_t packet_in = {
-		93372036854775807ULL,
-	}73.0,
-	}101.0,
-	}129.0,
-	}"UVWXYZABC",
-	};
-	mavlink_debug_vect_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_usec = packet_in.time_usec;
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        
-        	mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*10);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_debug_vect_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_debug_vect_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_debug_vect_pack(system_id, component_id, &msg , packet1.name , packet1.time_usec , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_debug_vect_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_debug_vect_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.name , packet1.time_usec , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_debug_vect_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_debug_vect_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_debug_vect_send(MAVLINK_COMM_1 , packet1.name , packet1.time_usec , packet1.x , packet1.y , packet1.z );
-	mavlink_msg_debug_vect_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_named_value_float(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_named_value_float_t packet_in = {
-		963497464,
-	}45.0,
-	}"IJKLMNOPQ",
-	};
-	mavlink_named_value_float_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.value = packet_in.value;
-        
-        	mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*10);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_named_value_float_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_named_value_float_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_named_value_float_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.name , packet1.value );
-	mavlink_msg_named_value_float_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_named_value_float_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.name , packet1.value );
-	mavlink_msg_named_value_float_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_named_value_float_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.name , packet1.value );
-	mavlink_msg_named_value_float_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_named_value_int(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_named_value_int_t packet_in = {
-		963497464,
-	}963497672,
-	}"IJKLMNOPQ",
-	};
-	mavlink_named_value_int_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.value = packet_in.value;
-        
-        	mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*10);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_named_value_int_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_named_value_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_named_value_int_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.name , packet1.value );
-	mavlink_msg_named_value_int_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_named_value_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.name , packet1.value );
-	mavlink_msg_named_value_int_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_named_value_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_named_value_int_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.name , packet1.value );
-	mavlink_msg_named_value_int_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_statustext(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_statustext_t packet_in = {
-		5,
-	}"BCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWX",
-	};
-	mavlink_statustext_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.severity = packet_in.severity;
-        
-        	mav_array_memcpy(packet1.text, packet_in.text, sizeof(char)*50);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_statustext_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_statustext_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_statustext_pack(system_id, component_id, &msg , packet1.severity , packet1.text );
-	mavlink_msg_statustext_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_statustext_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.severity , packet1.text );
-	mavlink_msg_statustext_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_statustext_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_statustext_send(MAVLINK_COMM_1 , packet1.severity , packet1.text );
-	mavlink_msg_statustext_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_debug(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_debug_t packet_in = {
-		963497464,
-	}45.0,
-	}29,
-	};
-	mavlink_debug_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.value = packet_in.value;
-        	packet1.ind = packet_in.ind;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_debug_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_debug_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_debug_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.ind , packet1.value );
-	mavlink_msg_debug_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_debug_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.ind , packet1.value );
-	mavlink_msg_debug_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_debug_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_debug_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.ind , packet1.value );
-	mavlink_msg_debug_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_heartbeat(system_id, component_id, last_msg);
-	mavlink_test_sys_status(system_id, component_id, last_msg);
-	mavlink_test_system_time(system_id, component_id, last_msg);
-	mavlink_test_ping(system_id, component_id, last_msg);
-	mavlink_test_change_operator_control(system_id, component_id, last_msg);
-	mavlink_test_change_operator_control_ack(system_id, component_id, last_msg);
-	mavlink_test_auth_key(system_id, component_id, last_msg);
-	mavlink_test_set_mode(system_id, component_id, last_msg);
-	mavlink_test_param_request_read(system_id, component_id, last_msg);
-	mavlink_test_param_request_list(system_id, component_id, last_msg);
-	mavlink_test_param_value(system_id, component_id, last_msg);
-	mavlink_test_param_set(system_id, component_id, last_msg);
-	mavlink_test_gps_raw_int(system_id, component_id, last_msg);
-	mavlink_test_gps_status(system_id, component_id, last_msg);
-	mavlink_test_scaled_imu(system_id, component_id, last_msg);
-	mavlink_test_raw_imu(system_id, component_id, last_msg);
-	mavlink_test_raw_pressure(system_id, component_id, last_msg);
-	mavlink_test_scaled_pressure(system_id, component_id, last_msg);
-	mavlink_test_attitude(system_id, component_id, last_msg);
-	mavlink_test_attitude_quaternion(system_id, component_id, last_msg);
-	mavlink_test_local_position_ned(system_id, component_id, last_msg);
-	mavlink_test_global_position_int(system_id, component_id, last_msg);
-	mavlink_test_rc_channels_scaled(system_id, component_id, last_msg);
-	mavlink_test_rc_channels_raw(system_id, component_id, last_msg);
-	mavlink_test_servo_output_raw(system_id, component_id, last_msg);
-	mavlink_test_mission_request_partial_list(system_id, component_id, last_msg);
-	mavlink_test_mission_write_partial_list(system_id, component_id, last_msg);
-	mavlink_test_mission_item(system_id, component_id, last_msg);
-	mavlink_test_mission_request(system_id, component_id, last_msg);
-	mavlink_test_mission_set_current(system_id, component_id, last_msg);
-	mavlink_test_mission_current(system_id, component_id, last_msg);
-	mavlink_test_mission_request_list(system_id, component_id, last_msg);
-	mavlink_test_mission_count(system_id, component_id, last_msg);
-	mavlink_test_mission_clear_all(system_id, component_id, last_msg);
-	mavlink_test_mission_item_reached(system_id, component_id, last_msg);
-	mavlink_test_mission_ack(system_id, component_id, last_msg);
-	mavlink_test_set_gps_global_origin(system_id, component_id, last_msg);
-	mavlink_test_gps_global_origin(system_id, component_id, last_msg);
-	mavlink_test_set_local_position_setpoint(system_id, component_id, last_msg);
-	mavlink_test_local_position_setpoint(system_id, component_id, last_msg);
-	mavlink_test_global_position_setpoint_int(system_id, component_id, last_msg);
-	mavlink_test_set_global_position_setpoint_int(system_id, component_id, last_msg);
-	mavlink_test_safety_set_allowed_area(system_id, component_id, last_msg);
-	mavlink_test_safety_allowed_area(system_id, component_id, last_msg);
-	mavlink_test_set_roll_pitch_yaw_thrust(system_id, component_id, last_msg);
-	mavlink_test_set_roll_pitch_yaw_speed_thrust(system_id, component_id, last_msg);
-	mavlink_test_roll_pitch_yaw_thrust_setpoint(system_id, component_id, last_msg);
-	mavlink_test_roll_pitch_yaw_speed_thrust_setpoint(system_id, component_id, last_msg);
-	mavlink_test_set_quad_motors_setpoint(system_id, component_id, last_msg);
-	mavlink_test_set_quad_swarm_roll_pitch_yaw_thrust(system_id, component_id, last_msg);
-	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_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);
-	mavlink_test_rc_channels_override(system_id, component_id, last_msg);
-	mavlink_test_vfr_hud(system_id, component_id, last_msg);
-	mavlink_test_command_long(system_id, component_id, last_msg);
-	mavlink_test_command_ack(system_id, component_id, last_msg);
-	mavlink_test_roll_pitch_yaw_rates_thrust_setpoint(system_id, component_id, last_msg);
-	mavlink_test_manual_setpoint(system_id, component_id, last_msg);
-	mavlink_test_local_position_ned_system_global_offset(system_id, component_id, last_msg);
-	mavlink_test_hil_state(system_id, component_id, last_msg);
-	mavlink_test_hil_controls(system_id, component_id, last_msg);
-	mavlink_test_hil_rc_inputs_raw(system_id, component_id, last_msg);
-	mavlink_test_optical_flow(system_id, component_id, last_msg);
-	mavlink_test_global_vision_position_estimate(system_id, component_id, last_msg);
-	mavlink_test_vision_position_estimate(system_id, component_id, last_msg);
-	mavlink_test_vision_speed_estimate(system_id, component_id, last_msg);
-	mavlink_test_vicon_position_estimate(system_id, component_id, last_msg);
-	mavlink_test_highres_imu(system_id, component_id, last_msg);
-	mavlink_test_omnidirectional_flow(system_id, component_id, last_msg);
-	mavlink_test_hil_sensor(system_id, component_id, last_msg);
-	mavlink_test_sim_state(system_id, component_id, last_msg);
-	mavlink_test_radio_status(system_id, component_id, last_msg);
-	mavlink_test_file_transfer_start(system_id, component_id, last_msg);
-	mavlink_test_file_transfer_dir_list(system_id, component_id, last_msg);
-	mavlink_test_file_transfer_res(system_id, component_id, last_msg);
-	mavlink_test_hil_gps(system_id, component_id, last_msg);
-	mavlink_test_hil_optical_flow(system_id, component_id, last_msg);
-	mavlink_test_hil_state_quaternion(system_id, component_id, last_msg);
-	mavlink_test_scaled_imu2(system_id, component_id, last_msg);
-	mavlink_test_log_request_list(system_id, component_id, last_msg);
-	mavlink_test_log_entry(system_id, component_id, last_msg);
-	mavlink_test_log_request_data(system_id, component_id, last_msg);
-	mavlink_test_log_data(system_id, component_id, last_msg);
-	mavlink_test_log_erase(system_id, component_id, last_msg);
-	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_data_transmission_handshake(system_id, component_id, last_msg);
-	mavlink_test_encapsulated_data(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);
-	mavlink_test_memory_vect(system_id, component_id, last_msg);
-	mavlink_test_debug_vect(system_id, component_id, last_msg);
-	mavlink_test_named_value_float(system_id, component_id, last_msg);
-	mavlink_test_named_value_int(system_id, component_id, last_msg);
-	mavlink_test_statustext(system_id, component_id, last_msg);
-	mavlink_test_debug(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // COMMON_TESTSUITE_H
diff --git a/mavlink/include/mavlink/v1.0/common/version.h b/mavlink/include/mavlink/v1.0/common/version.h
deleted file mode 100644
index 2f3736f45..000000000
--- a/mavlink/include/mavlink/v1.0/common/version.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from common.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:29 2014"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
- 
-#endif // MAVLINK_VERSION_H
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
deleted file mode 100644
index 2f15d31c4..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
+++ /dev/null
@@ -1,133 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol generated from matrixpilot.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef MATRIXPILOT_H
-#define MATRIXPILOT_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// 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}
-#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}
-#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}}}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_MATRIXPILOT
-
-// ENUM DEFINITIONS
-
-
-/** @brief Action required when performing CMD_PREFLIGHT_STORAGE */
-#ifndef HAVE_ENUM_MAV_PREFLIGHT_STORAGE_ACTION
-#define HAVE_ENUM_MAV_PREFLIGHT_STORAGE_ACTION
-enum MAV_PREFLIGHT_STORAGE_ACTION
-{
-	MAV_PFS_CMD_READ_ALL=0, /* Read all parameters from storage | */
-	MAV_PFS_CMD_WRITE_ALL=1, /* Write all parameters to storage | */
-	MAV_PFS_CMD_CLEAR_ALL=2, /* Clear all  parameters in storage | */
-	MAV_PFS_CMD_READ_SPECIFIC=3, /* Read specific parameters from storage | */
-	MAV_PFS_CMD_WRITE_SPECIFIC=4, /* Write specific parameters to storage | */
-	MAV_PFS_CMD_CLEAR_SPECIFIC=5, /* Clear specific parameters in storage | */
-	MAV_PFS_CMD_DO_NOTHING=6, /* do nothing | */
-	MAV_PREFLIGHT_STORAGE_ACTION_ENUM_END=7, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_CMD
-#define HAVE_ENUM_MAV_CMD
-enum MAV_CMD
-{
-	MAV_CMD_PREFLIGHT_STORAGE_ADVANCED=0, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Storage action: Action defined by MAV_PREFLIGHT_STORAGE_ACTION_ADVANCED| Storage area as defined by parameter database| Storage flags as defined by parameter database| Empty| Empty| Empty| Empty|  */
-	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_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
-
-#include "../common/common.h"
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_flexifunction_set.h"
-#include "./mavlink_msg_flexifunction_read_req.h"
-#include "./mavlink_msg_flexifunction_buffer_function.h"
-#include "./mavlink_msg_flexifunction_buffer_function_ack.h"
-#include "./mavlink_msg_flexifunction_directory.h"
-#include "./mavlink_msg_flexifunction_directory_ack.h"
-#include "./mavlink_msg_flexifunction_command.h"
-#include "./mavlink_msg_flexifunction_command_ack.h"
-#include "./mavlink_msg_serial_udb_extra_f2_a.h"
-#include "./mavlink_msg_serial_udb_extra_f2_b.h"
-#include "./mavlink_msg_serial_udb_extra_f4.h"
-#include "./mavlink_msg_serial_udb_extra_f5.h"
-#include "./mavlink_msg_serial_udb_extra_f6.h"
-#include "./mavlink_msg_serial_udb_extra_f7.h"
-#include "./mavlink_msg_serial_udb_extra_f8.h"
-#include "./mavlink_msg_serial_udb_extra_f13.h"
-#include "./mavlink_msg_serial_udb_extra_f14.h"
-#include "./mavlink_msg_serial_udb_extra_f15.h"
-#include "./mavlink_msg_serial_udb_extra_f16.h"
-#include "./mavlink_msg_altitudes.h"
-#include "./mavlink_msg_airspeeds.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // MATRIXPILOT_H
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h
deleted file mode 100644
index 7e62a9719..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from matrixpilot.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "matrixpilot.h"
-
-#endif // MAVLINK_H
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h
deleted file mode 100644
index bc47a547a..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE AIRSPEEDS PACKING
-
-#define MAVLINK_MSG_ID_AIRSPEEDS 182
-
-typedef struct __mavlink_airspeeds_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- int16_t airspeed_imu; ///< Airspeed estimate from IMU, cm/s
- int16_t airspeed_pitot; ///< Pitot measured forward airpseed, cm/s
- int16_t airspeed_hot_wire; ///< Hot wire anenometer measured airspeed, cm/s
- int16_t airspeed_ultrasonic; ///< Ultrasonic measured airspeed, cm/s
- int16_t aoa; ///< Angle of attack sensor, degrees * 10
- int16_t aoy; ///< Yaw angle sensor, degrees * 10
-} mavlink_airspeeds_t;
-
-#define MAVLINK_MSG_ID_AIRSPEEDS_LEN 16
-#define MAVLINK_MSG_ID_182_LEN 16
-
-#define MAVLINK_MSG_ID_AIRSPEEDS_CRC 154
-#define MAVLINK_MSG_ID_182_CRC 154
-
-
-
-#define MAVLINK_MESSAGE_INFO_AIRSPEEDS { \
-	"AIRSPEEDS", \
-	7, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_airspeeds_t, time_boot_ms) }, \
-         { "airspeed_imu", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_airspeeds_t, airspeed_imu) }, \
-         { "airspeed_pitot", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_airspeeds_t, airspeed_pitot) }, \
-         { "airspeed_hot_wire", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_airspeeds_t, airspeed_hot_wire) }, \
-         { "airspeed_ultrasonic", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_airspeeds_t, airspeed_ultrasonic) }, \
-         { "aoa", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_airspeeds_t, aoa) }, \
-         { "aoy", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_airspeeds_t, aoy) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a airspeeds 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 airspeed_imu Airspeed estimate from IMU, cm/s
- * @param airspeed_pitot Pitot measured forward airpseed, cm/s
- * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s
- * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s
- * @param aoa Angle of attack sensor, degrees * 10
- * @param aoy Yaw angle sensor, degrees * 10
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_AIRSPEEDS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
-#else
-	mavlink_airspeeds_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AIRSPEEDS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a airspeeds 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 airspeed_imu Airspeed estimate from IMU, cm/s
- * @param airspeed_pitot Pitot measured forward airpseed, cm/s
- * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s
- * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s
- * @param aoa Angle of attack sensor, degrees * 10
- * @param aoy Yaw angle sensor, degrees * 10
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_AIRSPEEDS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
-#else
-	mavlink_airspeeds_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AIRSPEEDS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a airspeeds 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 airspeeds C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_airspeeds_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds)
-{
-	return mavlink_msg_airspeeds_pack(system_id, component_id, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy);
-}
-
-/**
- * @brief Encode a airspeeds 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 airspeeds C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_airspeeds_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds)
-{
-	return mavlink_msg_airspeeds_pack_chan(system_id, component_id, chan, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy);
-}
-
-/**
- * @brief Send a airspeeds message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param airspeed_imu Airspeed estimate from IMU, cm/s
- * @param airspeed_pitot Pitot measured forward airpseed, cm/s
- * @param airspeed_hot_wire Hot wire anenometer measured airspeed, cm/s
- * @param airspeed_ultrasonic Ultrasonic measured airspeed, cm/s
- * @param aoa Angle of attack sensor, degrees * 10
- * @param aoy Yaw angle sensor, degrees * 10
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_airspeeds_send(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[MAVLINK_MSG_ID_AIRSPEEDS_LEN];
-	_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;
-	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
-
-// MESSAGE AIRSPEEDS UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from airspeeds message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_airspeeds_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field airspeed_imu from airspeeds message
- *
- * @return Airspeed estimate from IMU, cm/s
- */
-static inline int16_t mavlink_msg_airspeeds_get_airspeed_imu(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  4);
-}
-
-/**
- * @brief Get field airspeed_pitot from airspeeds message
- *
- * @return Pitot measured forward airpseed, cm/s
- */
-static inline int16_t mavlink_msg_airspeeds_get_airspeed_pitot(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  6);
-}
-
-/**
- * @brief Get field airspeed_hot_wire from airspeeds message
- *
- * @return Hot wire anenometer measured airspeed, cm/s
- */
-static inline int16_t mavlink_msg_airspeeds_get_airspeed_hot_wire(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field airspeed_ultrasonic from airspeeds message
- *
- * @return Ultrasonic measured airspeed, cm/s
- */
-static inline int16_t mavlink_msg_airspeeds_get_airspeed_ultrasonic(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  10);
-}
-
-/**
- * @brief Get field aoa from airspeeds message
- *
- * @return Angle of attack sensor, degrees * 10
- */
-static inline int16_t mavlink_msg_airspeeds_get_aoa(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Get field aoy from airspeeds message
- *
- * @return Yaw angle sensor, degrees * 10
- */
-static inline int16_t mavlink_msg_airspeeds_get_aoy(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  14);
-}
-
-/**
- * @brief Decode a airspeeds message into a struct
- *
- * @param msg The message to decode
- * @param airspeeds C-struct to decode the message contents into
- */
-static inline void mavlink_msg_airspeeds_decode(const mavlink_message_t* msg, mavlink_airspeeds_t* airspeeds)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	airspeeds->time_boot_ms = mavlink_msg_airspeeds_get_time_boot_ms(msg);
-	airspeeds->airspeed_imu = mavlink_msg_airspeeds_get_airspeed_imu(msg);
-	airspeeds->airspeed_pitot = mavlink_msg_airspeeds_get_airspeed_pitot(msg);
-	airspeeds->airspeed_hot_wire = mavlink_msg_airspeeds_get_airspeed_hot_wire(msg);
-	airspeeds->airspeed_ultrasonic = mavlink_msg_airspeeds_get_airspeed_ultrasonic(msg);
-	airspeeds->aoa = mavlink_msg_airspeeds_get_aoa(msg);
-	airspeeds->aoy = mavlink_msg_airspeeds_get_aoy(msg);
-#else
-	memcpy(airspeeds, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEEDS_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h
deleted file mode 100644
index e64787cc7..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE ALTITUDES PACKING
-
-#define MAVLINK_MSG_ID_ALTITUDES 181
-
-typedef struct __mavlink_altitudes_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- int32_t alt_gps; ///< GPS altitude in meters, expressed as * 1000 (millimeters), above MSL
- int32_t alt_imu; ///< IMU altitude above ground in meters, expressed as * 1000 (millimeters)
- int32_t alt_barometric; ///< barometeric altitude above ground in meters, expressed as * 1000 (millimeters)
- int32_t alt_optical_flow; ///< Optical flow altitude above ground in meters, expressed as * 1000 (millimeters)
- int32_t alt_range_finder; ///< Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters)
- int32_t alt_extra; ///< Extra altitude above ground in meters, expressed as * 1000 (millimeters)
-} mavlink_altitudes_t;
-
-#define MAVLINK_MSG_ID_ALTITUDES_LEN 28
-#define MAVLINK_MSG_ID_181_LEN 28
-
-#define MAVLINK_MSG_ID_ALTITUDES_CRC 55
-#define MAVLINK_MSG_ID_181_CRC 55
-
-
-
-#define MAVLINK_MESSAGE_INFO_ALTITUDES { \
-	"ALTITUDES", \
-	7, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_altitudes_t, time_boot_ms) }, \
-         { "alt_gps", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_altitudes_t, alt_gps) }, \
-         { "alt_imu", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_altitudes_t, alt_imu) }, \
-         { "alt_barometric", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_altitudes_t, alt_barometric) }, \
-         { "alt_optical_flow", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_altitudes_t, alt_optical_flow) }, \
-         { "alt_range_finder", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_altitudes_t, alt_range_finder) }, \
-         { "alt_extra", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_altitudes_t, alt_extra) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a altitudes 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 alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL
- * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_ALTITUDES_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDES_LEN);
-#else
-	mavlink_altitudes_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDES_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ALTITUDES;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ALTITUDES_LEN);
-#endif
-}
-
-/**
- * @brief Pack a altitudes 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 alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL
- * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_ALTITUDES_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDES_LEN);
-#else
-	mavlink_altitudes_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDES_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ALTITUDES;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ALTITUDES_LEN);
-#endif
-}
-
-/**
- * @brief Encode a altitudes 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 altitudes C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_altitudes_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes)
-{
-	return mavlink_msg_altitudes_pack(system_id, component_id, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra);
-}
-
-/**
- * @brief Encode a altitudes 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 altitudes C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_altitudes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes)
-{
-	return mavlink_msg_altitudes_pack_chan(system_id, component_id, chan, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra);
-}
-
-/**
- * @brief Send a altitudes message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL
- * @param alt_imu IMU altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_barometric barometeric altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_optical_flow Optical flow altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_range_finder Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters)
- * @param alt_extra Extra altitude above ground in meters, expressed as * 1000 (millimeters)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_altitudes_send(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[MAVLINK_MSG_ID_ALTITUDES_LEN];
-	_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;
-	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
-
-// MESSAGE ALTITUDES UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from altitudes message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_altitudes_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field alt_gps from altitudes message
- *
- * @return GPS altitude in meters, expressed as * 1000 (millimeters), above MSL
- */
-static inline int32_t mavlink_msg_altitudes_get_alt_gps(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field alt_imu from altitudes message
- *
- * @return IMU altitude above ground in meters, expressed as * 1000 (millimeters)
- */
-static inline int32_t mavlink_msg_altitudes_get_alt_imu(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field alt_barometric from altitudes message
- *
- * @return barometeric altitude above ground in meters, expressed as * 1000 (millimeters)
- */
-static inline int32_t mavlink_msg_altitudes_get_alt_barometric(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  12);
-}
-
-/**
- * @brief Get field alt_optical_flow from altitudes message
- *
- * @return Optical flow altitude above ground in meters, expressed as * 1000 (millimeters)
- */
-static inline int32_t mavlink_msg_altitudes_get_alt_optical_flow(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  16);
-}
-
-/**
- * @brief Get field alt_range_finder from altitudes message
- *
- * @return Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters)
- */
-static inline int32_t mavlink_msg_altitudes_get_alt_range_finder(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  20);
-}
-
-/**
- * @brief Get field alt_extra from altitudes message
- *
- * @return Extra altitude above ground in meters, expressed as * 1000 (millimeters)
- */
-static inline int32_t mavlink_msg_altitudes_get_alt_extra(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  24);
-}
-
-/**
- * @brief Decode a altitudes message into a struct
- *
- * @param msg The message to decode
- * @param altitudes C-struct to decode the message contents into
- */
-static inline void mavlink_msg_altitudes_decode(const mavlink_message_t* msg, mavlink_altitudes_t* altitudes)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	altitudes->time_boot_ms = mavlink_msg_altitudes_get_time_boot_ms(msg);
-	altitudes->alt_gps = mavlink_msg_altitudes_get_alt_gps(msg);
-	altitudes->alt_imu = mavlink_msg_altitudes_get_alt_imu(msg);
-	altitudes->alt_barometric = mavlink_msg_altitudes_get_alt_barometric(msg);
-	altitudes->alt_optical_flow = mavlink_msg_altitudes_get_alt_optical_flow(msg);
-	altitudes->alt_range_finder = mavlink_msg_altitudes_get_alt_range_finder(msg);
-	altitudes->alt_extra = mavlink_msg_altitudes_get_alt_extra(msg);
-#else
-	memcpy(altitudes, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ALTITUDES_LEN);
-#endif
-}
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
deleted file mode 100644
index 581bd35dc..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h
+++ /dev/null
@@ -1,303 +0,0 @@
-// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION PACKING
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION 152
-
-typedef struct __mavlink_flexifunction_buffer_function_t
-{
- uint16_t func_index; ///< Function index
- uint16_t func_count; ///< Total count of functions
- uint16_t data_address; ///< Address in the flexifunction data, Set to 0xFFFF to use address in target memory
- uint16_t data_size; ///< Size of the 
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- int8_t data[48]; ///< Settings data
-} mavlink_flexifunction_buffer_function_t;
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN 58
-#define MAVLINK_MSG_ID_152_LEN 58
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC 101
-#define MAVLINK_MSG_ID_152_CRC 101
-
-#define MAVLINK_MSG_FLEXIFUNCTION_BUFFER_FUNCTION_FIELD_DATA_LEN 48
-
-#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION { \
-	"FLEXIFUNCTION_BUFFER_FUNCTION", \
-	7, \
-	{  { "func_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_buffer_function_t, func_index) }, \
-         { "func_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_buffer_function_t, func_count) }, \
-         { "data_address", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_flexifunction_buffer_function_t, data_address) }, \
-         { "data_size", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_flexifunction_buffer_function_t, data_size) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_flexifunction_buffer_function_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_flexifunction_buffer_function_t, target_component) }, \
-         { "data", NULL, MAVLINK_TYPE_INT8_T, 48, 10, offsetof(mavlink_flexifunction_buffer_function_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a flexifunction_buffer_function 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 target_system System ID
- * @param target_component Component ID
- * @param func_index Function index
- * @param func_count Total count of functions
- * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory
- * @param data_size Size of the 
- * @param data Settings data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
-#else
-	mavlink_flexifunction_buffer_function_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
-#endif
-}
-
-/**
- * @brief Pack a flexifunction_buffer_function 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 target_system System ID
- * @param target_component Component ID
- * @param func_index Function index
- * @param func_count Total count of functions
- * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory
- * @param data_size Size of the 
- * @param data Settings data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
-#else
-	mavlink_flexifunction_buffer_function_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
-#endif
-}
-
-/**
- * @brief Encode a flexifunction_buffer_function 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 flexifunction_buffer_function C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function)
-{
-	return mavlink_msg_flexifunction_buffer_function_pack(system_id, component_id, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data);
-}
-
-/**
- * @brief Encode a flexifunction_buffer_function 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 flexifunction_buffer_function C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function)
-{
-	return mavlink_msg_flexifunction_buffer_function_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data);
-}
-
-/**
- * @brief Send a flexifunction_buffer_function message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param func_index Function index
- * @param func_count Total count of functions
- * @param data_address Address in the flexifunction data, Set to 0xFFFF to use address in target memory
- * @param data_size Size of the 
- * @param data Settings data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_flexifunction_buffer_function_send(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[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN];
-	_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;
-	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
-
-// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION UNPACKING
-
-
-/**
- * @brief Get field target_system from flexifunction_buffer_function message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_flexifunction_buffer_function_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field target_component from flexifunction_buffer_function message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_flexifunction_buffer_function_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Get field func_index from flexifunction_buffer_function message
- *
- * @return Function index
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_func_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field func_count from flexifunction_buffer_function message
- *
- * @return Total count of functions
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_func_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Get field data_address from flexifunction_buffer_function message
- *
- * @return Address in the flexifunction data, Set to 0xFFFF to use address in target memory
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data_address(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field data_size from flexifunction_buffer_function message
- *
- * @return Size of the 
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data_size(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field data from flexifunction_buffer_function message
- *
- * @return Settings data
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_get_data(const mavlink_message_t* msg, int8_t *data)
-{
-	return _MAV_RETURN_int8_t_array(msg, data, 48,  10);
-}
-
-/**
- * @brief Decode a flexifunction_buffer_function message into a struct
- *
- * @param msg The message to decode
- * @param flexifunction_buffer_function C-struct to decode the message contents into
- */
-static inline void mavlink_msg_flexifunction_buffer_function_decode(const mavlink_message_t* msg, mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	flexifunction_buffer_function->func_index = mavlink_msg_flexifunction_buffer_function_get_func_index(msg);
-	flexifunction_buffer_function->func_count = mavlink_msg_flexifunction_buffer_function_get_func_count(msg);
-	flexifunction_buffer_function->data_address = mavlink_msg_flexifunction_buffer_function_get_data_address(msg);
-	flexifunction_buffer_function->data_size = mavlink_msg_flexifunction_buffer_function_get_data_size(msg);
-	flexifunction_buffer_function->target_system = mavlink_msg_flexifunction_buffer_function_get_target_system(msg);
-	flexifunction_buffer_function->target_component = mavlink_msg_flexifunction_buffer_function_get_target_component(msg);
-	mavlink_msg_flexifunction_buffer_function_get_data(msg, flexifunction_buffer_function->data);
-#else
-	memcpy(flexifunction_buffer_function, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
-#endif
-}
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
deleted file mode 100644
index 790afd52b..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK PACKING
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK 153
-
-typedef struct __mavlink_flexifunction_buffer_function_ack_t
-{
- uint16_t func_index; ///< Function index
- uint16_t result; ///< result of acknowledge, 0=fail, 1=good
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_flexifunction_buffer_function_ack_t;
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN 6
-#define MAVLINK_MSG_ID_153_LEN 6
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC 109
-#define MAVLINK_MSG_ID_153_CRC 109
-
-
-
-#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION_ACK { \
-	"FLEXIFUNCTION_BUFFER_FUNCTION_ACK", \
-	4, \
-	{  { "func_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_buffer_function_ack_t, func_index) }, \
-         { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_buffer_function_ack_t, result) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_buffer_function_ack_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_buffer_function_ack_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a flexifunction_buffer_function_ack 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 target_system System ID
- * @param target_component Component ID
- * @param func_index Function index
- * @param result result of acknowledge, 0=fail, 1=good
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
-#else
-	mavlink_flexifunction_buffer_function_ack_t packet;
-	packet.func_index = func_index;
-	packet.result = result;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Pack a flexifunction_buffer_function_ack 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 target_system System ID
- * @param target_component Component ID
- * @param func_index Function index
- * @param result result of acknowledge, 0=fail, 1=good
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
-#else
-	mavlink_flexifunction_buffer_function_ack_t packet;
-	packet.func_index = func_index;
-	packet.result = result;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Encode a flexifunction_buffer_function_ack 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 flexifunction_buffer_function_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack)
-{
-	return mavlink_msg_flexifunction_buffer_function_ack_pack(system_id, component_id, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result);
-}
-
-/**
- * @brief Encode a flexifunction_buffer_function_ack 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 flexifunction_buffer_function_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack)
-{
-	return mavlink_msg_flexifunction_buffer_function_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result);
-}
-
-/**
- * @brief Send a flexifunction_buffer_function_ack message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param func_index Function index
- * @param result result of acknowledge, 0=fail, 1=good
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_flexifunction_buffer_function_ack_send(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[MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN];
-	_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;
-	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
-
-// MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK UNPACKING
-
-
-/**
- * @brief Get field target_system from flexifunction_buffer_function_ack message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_flexifunction_buffer_function_ack_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field target_component from flexifunction_buffer_function_ack message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_flexifunction_buffer_function_ack_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field func_index from flexifunction_buffer_function_ack message
- *
- * @return Function index
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_get_func_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field result from flexifunction_buffer_function_ack message
- *
- * @return result of acknowledge, 0=fail, 1=good
- */
-static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_get_result(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a flexifunction_buffer_function_ack message into a struct
- *
- * @param msg The message to decode
- * @param flexifunction_buffer_function_ack C-struct to decode the message contents into
- */
-static inline void mavlink_msg_flexifunction_buffer_function_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	flexifunction_buffer_function_ack->func_index = mavlink_msg_flexifunction_buffer_function_ack_get_func_index(msg);
-	flexifunction_buffer_function_ack->result = mavlink_msg_flexifunction_buffer_function_ack_get_result(msg);
-	flexifunction_buffer_function_ack->target_system = mavlink_msg_flexifunction_buffer_function_ack_get_target_system(msg);
-	flexifunction_buffer_function_ack->target_component = mavlink_msg_flexifunction_buffer_function_ack_get_target_component(msg);
-#else
-	memcpy(flexifunction_buffer_function_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
-#endif
-}
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
deleted file mode 100644
index ce722c8a4..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h
+++ /dev/null
@@ -1,221 +0,0 @@
-// MESSAGE FLEXIFUNCTION_COMMAND PACKING
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND 157
-
-typedef struct __mavlink_flexifunction_command_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t command_type; ///< Flexifunction command type
-} mavlink_flexifunction_command_t;
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN 3
-#define MAVLINK_MSG_ID_157_LEN 3
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC 133
-#define MAVLINK_MSG_ID_157_CRC 133
-
-
-
-#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND { \
-	"FLEXIFUNCTION_COMMAND", \
-	3, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_command_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_command_t, target_component) }, \
-         { "command_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_command_t, command_type) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a flexifunction_command 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 target_system System ID
- * @param target_component Component ID
- * @param command_type Flexifunction command type
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t command_type)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, command_type);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
-#else
-	mavlink_flexifunction_command_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.command_type = command_type;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
-#endif
-}
-
-/**
- * @brief Pack a flexifunction_command 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 target_system System ID
- * @param target_component Component ID
- * @param command_type Flexifunction command type
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t command_type)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, command_type);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
-#else
-	mavlink_flexifunction_command_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.command_type = command_type;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
-#endif
-}
-
-/**
- * @brief Encode a flexifunction_command 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 flexifunction_command C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command)
-{
-	return mavlink_msg_flexifunction_command_pack(system_id, component_id, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type);
-}
-
-/**
- * @brief Encode a flexifunction_command 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 flexifunction_command C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command)
-{
-	return mavlink_msg_flexifunction_command_pack_chan(system_id, component_id, chan, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type);
-}
-
-/**
- * @brief Send a flexifunction_command message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param command_type Flexifunction command type
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_flexifunction_command_send(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[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN];
-	_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;
-	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
-
-// MESSAGE FLEXIFUNCTION_COMMAND UNPACKING
-
-
-/**
- * @brief Get field target_system from flexifunction_command message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_flexifunction_command_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from flexifunction_command message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_flexifunction_command_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field command_type from flexifunction_command message
- *
- * @return Flexifunction command type
- */
-static inline uint8_t mavlink_msg_flexifunction_command_get_command_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a flexifunction_command message into a struct
- *
- * @param msg The message to decode
- * @param flexifunction_command C-struct to decode the message contents into
- */
-static inline void mavlink_msg_flexifunction_command_decode(const mavlink_message_t* msg, mavlink_flexifunction_command_t* flexifunction_command)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	flexifunction_command->target_system = mavlink_msg_flexifunction_command_get_target_system(msg);
-	flexifunction_command->target_component = mavlink_msg_flexifunction_command_get_target_component(msg);
-	flexifunction_command->command_type = mavlink_msg_flexifunction_command_get_command_type(msg);
-#else
-	memcpy(flexifunction_command, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
-#endif
-}
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
deleted file mode 100644
index 070dc4bf8..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE FLEXIFUNCTION_COMMAND_ACK PACKING
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK 158
-
-typedef struct __mavlink_flexifunction_command_ack_t
-{
- uint16_t command_type; ///< Command acknowledged
- uint16_t result; ///< result of acknowledge
-} mavlink_flexifunction_command_ack_t;
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN 4
-#define MAVLINK_MSG_ID_158_LEN 4
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC 208
-#define MAVLINK_MSG_ID_158_CRC 208
-
-
-
-#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND_ACK { \
-	"FLEXIFUNCTION_COMMAND_ACK", \
-	2, \
-	{  { "command_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_command_ack_t, command_type) }, \
-         { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_flexifunction_command_ack_t, result) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a flexifunction_command_ack 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 command_type Command acknowledged
- * @param result result of acknowledge
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t command_type, uint16_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN];
-	_mav_put_uint16_t(buf, 0, command_type);
-	_mav_put_uint16_t(buf, 2, result);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
-#else
-	mavlink_flexifunction_command_ack_t packet;
-	packet.command_type = command_type;
-	packet.result = result;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Pack a flexifunction_command_ack 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 command_type Command acknowledged
- * @param result result of acknowledge
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t command_type,uint16_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN];
-	_mav_put_uint16_t(buf, 0, command_type);
-	_mav_put_uint16_t(buf, 2, result);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
-#else
-	mavlink_flexifunction_command_ack_t packet;
-	packet.command_type = command_type;
-	packet.result = result;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Encode a flexifunction_command_ack 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 flexifunction_command_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack)
-{
-	return mavlink_msg_flexifunction_command_ack_pack(system_id, component_id, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result);
-}
-
-/**
- * @brief Encode a flexifunction_command_ack 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 flexifunction_command_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack)
-{
-	return mavlink_msg_flexifunction_command_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result);
-}
-
-/**
- * @brief Send a flexifunction_command_ack message
- * @param chan MAVLink channel to send the message
- *
- * @param command_type Command acknowledged
- * @param result result of acknowledge
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_flexifunction_command_ack_send(mavlink_channel_t chan, uint16_t command_type, uint16_t result)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN];
-	_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;
-	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
-
-// MESSAGE FLEXIFUNCTION_COMMAND_ACK UNPACKING
-
-
-/**
- * @brief Get field command_type from flexifunction_command_ack message
- *
- * @return Command acknowledged
- */
-static inline uint16_t mavlink_msg_flexifunction_command_ack_get_command_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field result from flexifunction_command_ack message
- *
- * @return result of acknowledge
- */
-static inline uint16_t mavlink_msg_flexifunction_command_ack_get_result(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a flexifunction_command_ack message into a struct
- *
- * @param msg The message to decode
- * @param flexifunction_command_ack C-struct to decode the message contents into
- */
-static inline void mavlink_msg_flexifunction_command_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_command_ack_t* flexifunction_command_ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	flexifunction_command_ack->command_type = mavlink_msg_flexifunction_command_ack_get_command_type(msg);
-	flexifunction_command_ack->result = mavlink_msg_flexifunction_command_ack_get_result(msg);
-#else
-	memcpy(flexifunction_command_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
-#endif
-}
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
deleted file mode 100644
index ef262a6b1..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h
+++ /dev/null
@@ -1,281 +0,0 @@
-// MESSAGE FLEXIFUNCTION_DIRECTORY PACKING
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY 155
-
-typedef struct __mavlink_flexifunction_directory_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t directory_type; ///< 0=inputs, 1=outputs
- uint8_t start_index; ///< index of first directory entry to write
- uint8_t count; ///< count of directory entries to write
- int8_t directory_data[48]; ///< Settings data
-} mavlink_flexifunction_directory_t;
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN 53
-#define MAVLINK_MSG_ID_155_LEN 53
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC 12
-#define MAVLINK_MSG_ID_155_CRC 12
-
-#define MAVLINK_MSG_FLEXIFUNCTION_DIRECTORY_FIELD_DIRECTORY_DATA_LEN 48
-
-#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY { \
-	"FLEXIFUNCTION_DIRECTORY", \
-	6, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_directory_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_directory_t, target_component) }, \
-         { "directory_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_directory_t, directory_type) }, \
-         { "start_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_flexifunction_directory_t, start_index) }, \
-         { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_directory_t, count) }, \
-         { "directory_data", NULL, MAVLINK_TYPE_INT8_T, 48, 5, offsetof(mavlink_flexifunction_directory_t, directory_data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a flexifunction_directory 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 target_system System ID
- * @param target_component Component ID
- * @param directory_type 0=inputs, 1=outputs
- * @param start_index index of first directory entry to write
- * @param count count of directory entries to write
- * @param directory_data Settings data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
-#else
-	mavlink_flexifunction_directory_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
-#endif
-}
-
-/**
- * @brief Pack a flexifunction_directory 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 target_system System ID
- * @param target_component Component ID
- * @param directory_type 0=inputs, 1=outputs
- * @param start_index index of first directory entry to write
- * @param count count of directory entries to write
- * @param directory_data Settings data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
-#else
-	mavlink_flexifunction_directory_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
-#endif
-}
-
-/**
- * @brief Encode a flexifunction_directory 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 flexifunction_directory C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory)
-{
-	return mavlink_msg_flexifunction_directory_pack(system_id, component_id, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data);
-}
-
-/**
- * @brief Encode a flexifunction_directory 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 flexifunction_directory C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory)
-{
-	return mavlink_msg_flexifunction_directory_pack_chan(system_id, component_id, chan, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data);
-}
-
-/**
- * @brief Send a flexifunction_directory message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param directory_type 0=inputs, 1=outputs
- * @param start_index index of first directory entry to write
- * @param count count of directory entries to write
- * @param directory_data Settings data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_flexifunction_directory_send(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[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN];
-	_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;
-	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
-
-// MESSAGE FLEXIFUNCTION_DIRECTORY UNPACKING
-
-
-/**
- * @brief Get field target_system from flexifunction_directory message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from flexifunction_directory message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field directory_type from flexifunction_directory message
- *
- * @return 0=inputs, 1=outputs
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_get_directory_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field start_index from flexifunction_directory message
- *
- * @return index of first directory entry to write
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_get_start_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field count from flexifunction_directory message
- *
- * @return count of directory entries to write
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_get_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field directory_data from flexifunction_directory message
- *
- * @return Settings data
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_get_directory_data(const mavlink_message_t* msg, int8_t *directory_data)
-{
-	return _MAV_RETURN_int8_t_array(msg, directory_data, 48,  5);
-}
-
-/**
- * @brief Decode a flexifunction_directory message into a struct
- *
- * @param msg The message to decode
- * @param flexifunction_directory C-struct to decode the message contents into
- */
-static inline void mavlink_msg_flexifunction_directory_decode(const mavlink_message_t* msg, mavlink_flexifunction_directory_t* flexifunction_directory)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	flexifunction_directory->target_system = mavlink_msg_flexifunction_directory_get_target_system(msg);
-	flexifunction_directory->target_component = mavlink_msg_flexifunction_directory_get_target_component(msg);
-	flexifunction_directory->directory_type = mavlink_msg_flexifunction_directory_get_directory_type(msg);
-	flexifunction_directory->start_index = mavlink_msg_flexifunction_directory_get_start_index(msg);
-	flexifunction_directory->count = mavlink_msg_flexifunction_directory_get_count(msg);
-	mavlink_msg_flexifunction_directory_get_directory_data(msg, flexifunction_directory->directory_data);
-#else
-	memcpy(flexifunction_directory, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
-#endif
-}
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
deleted file mode 100644
index d3a386ce5..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE FLEXIFUNCTION_DIRECTORY_ACK PACKING
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK 156
-
-typedef struct __mavlink_flexifunction_directory_ack_t
-{
- uint16_t result; ///< result of acknowledge, 0=fail, 1=good
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t directory_type; ///< 0=inputs, 1=outputs
- uint8_t start_index; ///< index of first directory entry to write
- uint8_t count; ///< count of directory entries to write
-} mavlink_flexifunction_directory_ack_t;
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN 7
-#define MAVLINK_MSG_ID_156_LEN 7
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC 218
-#define MAVLINK_MSG_ID_156_CRC 218
-
-
-
-#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY_ACK { \
-	"FLEXIFUNCTION_DIRECTORY_ACK", \
-	6, \
-	{  { "result", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_flexifunction_directory_ack_t, result) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_flexifunction_directory_ack_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_flexifunction_directory_ack_t, target_component) }, \
-         { "directory_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_directory_ack_t, directory_type) }, \
-         { "start_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_directory_ack_t, start_index) }, \
-         { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_flexifunction_directory_ack_t, count) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a flexifunction_directory_ack 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 target_system System ID
- * @param target_component Component ID
- * @param directory_type 0=inputs, 1=outputs
- * @param start_index index of first directory entry to write
- * @param count count of directory entries to write
- * @param result result of acknowledge, 0=fail, 1=good
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
-#else
-	mavlink_flexifunction_directory_ack_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Pack a flexifunction_directory_ack 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 target_system System ID
- * @param target_component Component ID
- * @param directory_type 0=inputs, 1=outputs
- * @param start_index index of first directory entry to write
- * @param count count of directory entries to write
- * @param result result of acknowledge, 0=fail, 1=good
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
-#else
-	mavlink_flexifunction_directory_ack_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Encode a flexifunction_directory_ack 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 flexifunction_directory_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack)
-{
-	return mavlink_msg_flexifunction_directory_ack_pack(system_id, component_id, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result);
-}
-
-/**
- * @brief Encode a flexifunction_directory_ack 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 flexifunction_directory_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack)
-{
-	return mavlink_msg_flexifunction_directory_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result);
-}
-
-/**
- * @brief Send a flexifunction_directory_ack message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param directory_type 0=inputs, 1=outputs
- * @param start_index index of first directory entry to write
- * @param count count of directory entries to write
- * @param result result of acknowledge, 0=fail, 1=good
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_flexifunction_directory_ack_send(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[MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN];
-	_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;
-	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
-
-// MESSAGE FLEXIFUNCTION_DIRECTORY_ACK UNPACKING
-
-
-/**
- * @brief Get field target_system from flexifunction_directory_ack message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field target_component from flexifunction_directory_ack message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field directory_type from flexifunction_directory_ack message
- *
- * @return 0=inputs, 1=outputs
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_directory_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field start_index from flexifunction_directory_ack message
- *
- * @return index of first directory entry to write
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_start_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field count from flexifunction_directory_ack message
- *
- * @return count of directory entries to write
- */
-static inline uint8_t mavlink_msg_flexifunction_directory_ack_get_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field result from flexifunction_directory_ack message
- *
- * @return result of acknowledge, 0=fail, 1=good
- */
-static inline uint16_t mavlink_msg_flexifunction_directory_ack_get_result(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Decode a flexifunction_directory_ack message into a struct
- *
- * @param msg The message to decode
- * @param flexifunction_directory_ack C-struct to decode the message contents into
- */
-static inline void mavlink_msg_flexifunction_directory_ack_decode(const mavlink_message_t* msg, mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	flexifunction_directory_ack->result = mavlink_msg_flexifunction_directory_ack_get_result(msg);
-	flexifunction_directory_ack->target_system = mavlink_msg_flexifunction_directory_ack_get_target_system(msg);
-	flexifunction_directory_ack->target_component = mavlink_msg_flexifunction_directory_ack_get_target_component(msg);
-	flexifunction_directory_ack->directory_type = mavlink_msg_flexifunction_directory_ack_get_directory_type(msg);
-	flexifunction_directory_ack->start_index = mavlink_msg_flexifunction_directory_ack_get_start_index(msg);
-	flexifunction_directory_ack->count = mavlink_msg_flexifunction_directory_ack_get_count(msg);
-#else
-	memcpy(flexifunction_directory_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
-#endif
-}
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
deleted file mode 100644
index e50f77b08..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE FLEXIFUNCTION_READ_REQ PACKING
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ 151
-
-typedef struct __mavlink_flexifunction_read_req_t
-{
- int16_t read_req_type; ///< Type of flexifunction data requested
- int16_t data_index; ///< index into data where needed
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_flexifunction_read_req_t;
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN 6
-#define MAVLINK_MSG_ID_151_LEN 6
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC 26
-#define MAVLINK_MSG_ID_151_CRC 26
-
-
-
-#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_READ_REQ { \
-	"FLEXIFUNCTION_READ_REQ", \
-	4, \
-	{  { "read_req_type", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_flexifunction_read_req_t, read_req_type) }, \
-         { "data_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_flexifunction_read_req_t, data_index) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_flexifunction_read_req_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_flexifunction_read_req_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a flexifunction_read_req 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 target_system System ID
- * @param target_component Component ID
- * @param read_req_type Type of flexifunction data requested
- * @param data_index index into data where needed
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_read_req_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
-#else
-	mavlink_flexifunction_read_req_t packet;
-	packet.read_req_type = read_req_type;
-	packet.data_index = data_index;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
-#endif
-}
-
-/**
- * @brief Pack a flexifunction_read_req 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 target_system System ID
- * @param target_component Component ID
- * @param read_req_type Type of flexifunction data requested
- * @param data_index index into data where needed
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_read_req_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
-#else
-	mavlink_flexifunction_read_req_t packet;
-	packet.read_req_type = read_req_type;
-	packet.data_index = data_index;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
-#endif
-}
-
-/**
- * @brief Encode a flexifunction_read_req 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 flexifunction_read_req C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_read_req_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req)
-{
-	return mavlink_msg_flexifunction_read_req_pack(system_id, component_id, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index);
-}
-
-/**
- * @brief Encode a flexifunction_read_req 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 flexifunction_read_req C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_read_req_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req)
-{
-	return mavlink_msg_flexifunction_read_req_pack_chan(system_id, component_id, chan, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index);
-}
-
-/**
- * @brief Send a flexifunction_read_req message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param read_req_type Type of flexifunction data requested
- * @param data_index index into data where needed
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_flexifunction_read_req_send(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[MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN];
-	_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;
-	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
-
-// MESSAGE FLEXIFUNCTION_READ_REQ UNPACKING
-
-
-/**
- * @brief Get field target_system from flexifunction_read_req message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_flexifunction_read_req_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field target_component from flexifunction_read_req message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_flexifunction_read_req_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field read_req_type from flexifunction_read_req message
- *
- * @return Type of flexifunction data requested
- */
-static inline int16_t mavlink_msg_flexifunction_read_req_get_read_req_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  0);
-}
-
-/**
- * @brief Get field data_index from flexifunction_read_req message
- *
- * @return index into data where needed
- */
-static inline int16_t mavlink_msg_flexifunction_read_req_get_data_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  2);
-}
-
-/**
- * @brief Decode a flexifunction_read_req message into a struct
- *
- * @param msg The message to decode
- * @param flexifunction_read_req C-struct to decode the message contents into
- */
-static inline void mavlink_msg_flexifunction_read_req_decode(const mavlink_message_t* msg, mavlink_flexifunction_read_req_t* flexifunction_read_req)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	flexifunction_read_req->read_req_type = mavlink_msg_flexifunction_read_req_get_read_req_type(msg);
-	flexifunction_read_req->data_index = mavlink_msg_flexifunction_read_req_get_data_index(msg);
-	flexifunction_read_req->target_system = mavlink_msg_flexifunction_read_req_get_target_system(msg);
-	flexifunction_read_req->target_component = mavlink_msg_flexifunction_read_req_get_target_component(msg);
-#else
-	memcpy(flexifunction_read_req, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
-#endif
-}
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
deleted file mode 100644
index 41afb3811..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE FLEXIFUNCTION_SET PACKING
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET 150
-
-typedef struct __mavlink_flexifunction_set_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_flexifunction_set_t;
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN 2
-#define MAVLINK_MSG_ID_150_LEN 2
-
-#define MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC 181
-#define MAVLINK_MSG_ID_150_CRC 181
-
-
-
-#define MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_SET { \
-	"FLEXIFUNCTION_SET", \
-	2, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_flexifunction_set_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_flexifunction_set_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a flexifunction_set 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
-#else
-	mavlink_flexifunction_set_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_SET;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
-#endif
-}
-
-/**
- * @brief Pack a flexifunction_set 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 target_system System ID
- * @param target_component Component ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_flexifunction_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
-#else
-	mavlink_flexifunction_set_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FLEXIFUNCTION_SET;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
-#endif
-}
-
-/**
- * @brief Encode a flexifunction_set 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 flexifunction_set C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set)
-{
-	return mavlink_msg_flexifunction_set_pack(system_id, component_id, msg, flexifunction_set->target_system, flexifunction_set->target_component);
-}
-
-/**
- * @brief Encode a flexifunction_set 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 flexifunction_set C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_flexifunction_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set)
-{
-	return mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, chan, msg, flexifunction_set->target_system, flexifunction_set->target_component);
-}
-
-/**
- * @brief Send a flexifunction_set message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_flexifunction_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN];
-	_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;
-	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
-
-// MESSAGE FLEXIFUNCTION_SET UNPACKING
-
-
-/**
- * @brief Get field target_system from flexifunction_set message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_flexifunction_set_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from flexifunction_set message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_flexifunction_set_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Decode a flexifunction_set message into a struct
- *
- * @param msg The message to decode
- * @param flexifunction_set C-struct to decode the message contents into
- */
-static inline void mavlink_msg_flexifunction_set_decode(const mavlink_message_t* msg, mavlink_flexifunction_set_t* flexifunction_set)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	flexifunction_set->target_system = mavlink_msg_flexifunction_set_get_target_system(msg);
-	flexifunction_set->target_component = mavlink_msg_flexifunction_set_get_target_component(msg);
-#else
-	memcpy(flexifunction_set, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
-#endif
-}
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
deleted file mode 100644
index d21ab4816..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F13 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13 177
-
-typedef struct __mavlink_serial_udb_extra_f13_t
-{
- int32_t sue_lat_origin; ///< Serial UDB Extra MP Origin Latitude
- int32_t sue_lon_origin; ///< Serial UDB Extra MP Origin Longitude
- int32_t sue_alt_origin; ///< Serial UDB Extra MP Origin Altitude Above Sea Level
- int16_t sue_week_no; ///< Serial UDB Extra GPS Week Number
-} mavlink_serial_udb_extra_f13_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN 14
-#define MAVLINK_MSG_ID_177_LEN 14
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC 249
-#define MAVLINK_MSG_ID_177_CRC 249
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F13 { \
-	"SERIAL_UDB_EXTRA_F13", \
-	4, \
-	{  { "sue_lat_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f13_t, sue_lat_origin) }, \
-         { "sue_lon_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f13_t, sue_lon_origin) }, \
-         { "sue_alt_origin", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_serial_udb_extra_f13_t, sue_alt_origin) }, \
-         { "sue_week_no", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_serial_udb_extra_f13_t, sue_week_no) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f13 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 sue_week_no Serial UDB Extra GPS Week Number
- * @param sue_lat_origin Serial UDB Extra MP Origin Latitude
- * @param sue_lon_origin Serial UDB Extra MP Origin Longitude
- * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
-#else
-	mavlink_serial_udb_extra_f13_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f13 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 sue_week_no Serial UDB Extra GPS Week Number
- * @param sue_lat_origin Serial UDB Extra MP Origin Latitude
- * @param sue_lon_origin Serial UDB Extra MP Origin Longitude
- * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
-#else
-	mavlink_serial_udb_extra_f13_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f13 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_udb_extra_f13 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13)
-{
-	return mavlink_msg_serial_udb_extra_f13_pack(system_id, component_id, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f13 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_udb_extra_f13 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13)
-{
-	return mavlink_msg_serial_udb_extra_f13_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin);
-}
-
-/**
- * @brief Send a serial_udb_extra_f13 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_week_no Serial UDB Extra GPS Week Number
- * @param sue_lat_origin Serial UDB Extra MP Origin Latitude
- * @param sue_lon_origin Serial UDB Extra MP Origin Longitude
- * @param sue_alt_origin Serial UDB Extra MP Origin Altitude Above Sea Level
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f13_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F13 UNPACKING
-
-
-/**
- * @brief Get field sue_week_no from serial_udb_extra_f13 message
- *
- * @return Serial UDB Extra GPS Week Number
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f13_get_sue_week_no(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Get field sue_lat_origin from serial_udb_extra_f13 message
- *
- * @return Serial UDB Extra MP Origin Latitude
- */
-static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_lat_origin(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field sue_lon_origin from serial_udb_extra_f13 message
- *
- * @return Serial UDB Extra MP Origin Longitude
- */
-static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_lon_origin(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field sue_alt_origin from serial_udb_extra_f13 message
- *
- * @return Serial UDB Extra MP Origin Altitude Above Sea Level
- */
-static inline int32_t mavlink_msg_serial_udb_extra_f13_get_sue_alt_origin(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f13 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f13 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f13_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f13->sue_lat_origin = mavlink_msg_serial_udb_extra_f13_get_sue_lat_origin(msg);
-	serial_udb_extra_f13->sue_lon_origin = mavlink_msg_serial_udb_extra_f13_get_sue_lon_origin(msg);
-	serial_udb_extra_f13->sue_alt_origin = mavlink_msg_serial_udb_extra_f13_get_sue_alt_origin(msg);
-	serial_udb_extra_f13->sue_week_no = mavlink_msg_serial_udb_extra_f13_get_sue_week_no(msg);
-#else
-	memcpy(serial_udb_extra_f13, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
-#endif
-}
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
deleted file mode 100644
index 6ac12f28a..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h
+++ /dev/null
@@ -1,397 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F14 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14 178
-
-typedef struct __mavlink_serial_udb_extra_f14_t
-{
- uint32_t sue_TRAP_SOURCE; ///< Serial UDB Extra Type Program Address of Last Trap
- int16_t sue_RCON; ///< Serial UDB Extra Reboot Regitster of DSPIC
- int16_t sue_TRAP_FLAGS; ///< Serial UDB Extra  Last dspic Trap Flags
- int16_t sue_osc_fail_count; ///< Serial UDB Extra Number of Ocillator Failures
- uint8_t sue_WIND_ESTIMATION; ///< Serial UDB Extra Wind Estimation Enabled
- uint8_t sue_GPS_TYPE; ///< Serial UDB Extra Type of GPS Unit
- uint8_t sue_DR; ///< Serial UDB Extra Dead Reckoning Enabled
- uint8_t sue_BOARD_TYPE; ///< Serial UDB Extra Type of UDB Hardware
- uint8_t sue_AIRFRAME; ///< Serial UDB Extra Type of Airframe
- uint8_t sue_CLOCK_CONFIG; ///< Serial UDB Extra UDB Internal Clock Configuration
- uint8_t sue_FLIGHT_PLAN_TYPE; ///< Serial UDB Extra Type of Flight Plan
-} mavlink_serial_udb_extra_f14_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN 17
-#define MAVLINK_MSG_ID_178_LEN 17
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC 123
-#define MAVLINK_MSG_ID_178_CRC 123
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F14 { \
-	"SERIAL_UDB_EXTRA_F14", \
-	11, \
-	{  { "sue_TRAP_SOURCE", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f14_t, sue_TRAP_SOURCE) }, \
-         { "sue_RCON", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_serial_udb_extra_f14_t, sue_RCON) }, \
-         { "sue_TRAP_FLAGS", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_serial_udb_extra_f14_t, sue_TRAP_FLAGS) }, \
-         { "sue_osc_fail_count", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_serial_udb_extra_f14_t, sue_osc_fail_count) }, \
-         { "sue_WIND_ESTIMATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_serial_udb_extra_f14_t, sue_WIND_ESTIMATION) }, \
-         { "sue_GPS_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_serial_udb_extra_f14_t, sue_GPS_TYPE) }, \
-         { "sue_DR", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_serial_udb_extra_f14_t, sue_DR) }, \
-         { "sue_BOARD_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_serial_udb_extra_f14_t, sue_BOARD_TYPE) }, \
-         { "sue_AIRFRAME", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_serial_udb_extra_f14_t, sue_AIRFRAME) }, \
-         { "sue_CLOCK_CONFIG", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_serial_udb_extra_f14_t, sue_CLOCK_CONFIG) }, \
-         { "sue_FLIGHT_PLAN_TYPE", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_serial_udb_extra_f14_t, sue_FLIGHT_PLAN_TYPE) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f14 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 sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled
- * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit
- * @param sue_DR Serial UDB Extra Dead Reckoning Enabled
- * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware
- * @param sue_AIRFRAME Serial UDB Extra Type of Airframe
- * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC
- * @param sue_TRAP_FLAGS Serial UDB Extra  Last dspic Trap Flags
- * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap
- * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures
- * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration
- * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
-#else
-	mavlink_serial_udb_extra_f14_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f14 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 sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled
- * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit
- * @param sue_DR Serial UDB Extra Dead Reckoning Enabled
- * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware
- * @param sue_AIRFRAME Serial UDB Extra Type of Airframe
- * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC
- * @param sue_TRAP_FLAGS Serial UDB Extra  Last dspic Trap Flags
- * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap
- * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures
- * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration
- * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
-#else
-	mavlink_serial_udb_extra_f14_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f14 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_udb_extra_f14 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14)
-{
-	return mavlink_msg_serial_udb_extra_f14_pack(system_id, component_id, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f14 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_udb_extra_f14 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14)
-{
-	return mavlink_msg_serial_udb_extra_f14_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE);
-}
-
-/**
- * @brief Send a serial_udb_extra_f14 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled
- * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit
- * @param sue_DR Serial UDB Extra Dead Reckoning Enabled
- * @param sue_BOARD_TYPE Serial UDB Extra Type of UDB Hardware
- * @param sue_AIRFRAME Serial UDB Extra Type of Airframe
- * @param sue_RCON Serial UDB Extra Reboot Regitster of DSPIC
- * @param sue_TRAP_FLAGS Serial UDB Extra  Last dspic Trap Flags
- * @param sue_TRAP_SOURCE Serial UDB Extra Type Program Address of Last Trap
- * @param sue_osc_fail_count Serial UDB Extra Number of Ocillator Failures
- * @param sue_CLOCK_CONFIG Serial UDB Extra UDB Internal Clock Configuration
- * @param sue_FLIGHT_PLAN_TYPE Serial UDB Extra Type of Flight Plan
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f14_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F14 UNPACKING
-
-
-/**
- * @brief Get field sue_WIND_ESTIMATION from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Wind Estimation Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_WIND_ESTIMATION(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field sue_GPS_TYPE from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Type of GPS Unit
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_GPS_TYPE(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field sue_DR from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Dead Reckoning Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_DR(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field sue_BOARD_TYPE from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Type of UDB Hardware
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_BOARD_TYPE(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  13);
-}
-
-/**
- * @brief Get field sue_AIRFRAME from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Type of Airframe
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_AIRFRAME(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Get field sue_RCON from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Reboot Regitster of DSPIC
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_RCON(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  4);
-}
-
-/**
- * @brief Get field sue_TRAP_FLAGS from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra  Last dspic Trap Flags
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_FLAGS(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  6);
-}
-
-/**
- * @brief Get field sue_TRAP_SOURCE from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Type Program Address of Last Trap
- */
-static inline uint32_t mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_SOURCE(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field sue_osc_fail_count from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Number of Ocillator Failures
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f14_get_sue_osc_fail_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field sue_CLOCK_CONFIG from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra UDB Internal Clock Configuration
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_CLOCK_CONFIG(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  15);
-}
-
-/**
- * @brief Get field sue_FLIGHT_PLAN_TYPE from serial_udb_extra_f14 message
- *
- * @return Serial UDB Extra Type of Flight Plan
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f14_get_sue_FLIGHT_PLAN_TYPE(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f14 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f14 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f14_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f14->sue_TRAP_SOURCE = mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_SOURCE(msg);
-	serial_udb_extra_f14->sue_RCON = mavlink_msg_serial_udb_extra_f14_get_sue_RCON(msg);
-	serial_udb_extra_f14->sue_TRAP_FLAGS = mavlink_msg_serial_udb_extra_f14_get_sue_TRAP_FLAGS(msg);
-	serial_udb_extra_f14->sue_osc_fail_count = mavlink_msg_serial_udb_extra_f14_get_sue_osc_fail_count(msg);
-	serial_udb_extra_f14->sue_WIND_ESTIMATION = mavlink_msg_serial_udb_extra_f14_get_sue_WIND_ESTIMATION(msg);
-	serial_udb_extra_f14->sue_GPS_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_GPS_TYPE(msg);
-	serial_udb_extra_f14->sue_DR = mavlink_msg_serial_udb_extra_f14_get_sue_DR(msg);
-	serial_udb_extra_f14->sue_BOARD_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_BOARD_TYPE(msg);
-	serial_udb_extra_f14->sue_AIRFRAME = mavlink_msg_serial_udb_extra_f14_get_sue_AIRFRAME(msg);
-	serial_udb_extra_f14->sue_CLOCK_CONFIG = mavlink_msg_serial_udb_extra_f14_get_sue_CLOCK_CONFIG(msg);
-	serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE = mavlink_msg_serial_udb_extra_f14_get_sue_FLIGHT_PLAN_TYPE(msg);
-#else
-	memcpy(serial_udb_extra_f14, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
-#endif
-}
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
deleted file mode 100644
index 10c3f4ca4..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h
+++ /dev/null
@@ -1,200 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F15 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15 179
-
-typedef struct __mavlink_serial_udb_extra_f15_t
-{
- uint8_t sue_ID_VEHICLE_MODEL_NAME[40]; ///< Serial UDB Extra Model Name Of Vehicle
- uint8_t sue_ID_VEHICLE_REGISTRATION[20]; ///< Serial UDB Extra Registraton Number of Vehicle
-} mavlink_serial_udb_extra_f15_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN 60
-#define MAVLINK_MSG_ID_179_LEN 60
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC 7
-#define MAVLINK_MSG_ID_179_CRC 7
-
-#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F15_FIELD_SUE_ID_VEHICLE_MODEL_NAME_LEN 40
-#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F15_FIELD_SUE_ID_VEHICLE_REGISTRATION_LEN 20
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F15 { \
-	"SERIAL_UDB_EXTRA_F15", \
-	2, \
-	{  { "sue_ID_VEHICLE_MODEL_NAME", NULL, MAVLINK_TYPE_UINT8_T, 40, 0, offsetof(mavlink_serial_udb_extra_f15_t, sue_ID_VEHICLE_MODEL_NAME) }, \
-         { "sue_ID_VEHICLE_REGISTRATION", NULL, MAVLINK_TYPE_UINT8_T, 20, 40, offsetof(mavlink_serial_udb_extra_f15_t, sue_ID_VEHICLE_REGISTRATION) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f15 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 sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle
- * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN];
-
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
-#else
-	mavlink_serial_udb_extra_f15_t packet;
-
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f15 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 sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle
- * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN];
-
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
-#else
-	mavlink_serial_udb_extra_f15_t packet;
-
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f15 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_udb_extra_f15 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15)
-{
-	return mavlink_msg_serial_udb_extra_f15_pack(system_id, component_id, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f15 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_udb_extra_f15 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15)
-{
-	return mavlink_msg_serial_udb_extra_f15_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION);
-}
-
-/**
- * @brief Send a serial_udb_extra_f15 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle
- * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f15_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F15 UNPACKING
-
-
-/**
- * @brief Get field sue_ID_VEHICLE_MODEL_NAME from serial_udb_extra_f15 message
- *
- * @return Serial UDB Extra Model Name Of Vehicle
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_MODEL_NAME(const mavlink_message_t* msg, uint8_t *sue_ID_VEHICLE_MODEL_NAME)
-{
-	return _MAV_RETURN_uint8_t_array(msg, sue_ID_VEHICLE_MODEL_NAME, 40,  0);
-}
-
-/**
- * @brief Get field sue_ID_VEHICLE_REGISTRATION from serial_udb_extra_f15 message
- *
- * @return Serial UDB Extra Registraton Number of Vehicle
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_REGISTRATION(const mavlink_message_t* msg, uint8_t *sue_ID_VEHICLE_REGISTRATION)
-{
-	return _MAV_RETURN_uint8_t_array(msg, sue_ID_VEHICLE_REGISTRATION, 20,  40);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f15 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f15 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f15_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_MODEL_NAME(msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME);
-	mavlink_msg_serial_udb_extra_f15_get_sue_ID_VEHICLE_REGISTRATION(msg, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION);
-#else
-	memcpy(serial_udb_extra_f15, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
-#endif
-}
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
deleted file mode 100644
index 659e6b7c5..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h
+++ /dev/null
@@ -1,200 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F16 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16 180
-
-typedef struct __mavlink_serial_udb_extra_f16_t
-{
- uint8_t sue_ID_LEAD_PILOT[40]; ///< Serial UDB Extra Name of Expected Lead Pilot
- uint8_t sue_ID_DIY_DRONES_URL[70]; ///< Serial UDB Extra URL of Lead Pilot or Team
-} mavlink_serial_udb_extra_f16_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN 110
-#define MAVLINK_MSG_ID_180_LEN 110
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC 222
-#define MAVLINK_MSG_ID_180_CRC 222
-
-#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F16_FIELD_SUE_ID_LEAD_PILOT_LEN 40
-#define MAVLINK_MSG_SERIAL_UDB_EXTRA_F16_FIELD_SUE_ID_DIY_DRONES_URL_LEN 70
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F16 { \
-	"SERIAL_UDB_EXTRA_F16", \
-	2, \
-	{  { "sue_ID_LEAD_PILOT", NULL, MAVLINK_TYPE_UINT8_T, 40, 0, offsetof(mavlink_serial_udb_extra_f16_t, sue_ID_LEAD_PILOT) }, \
-         { "sue_ID_DIY_DRONES_URL", NULL, MAVLINK_TYPE_UINT8_T, 70, 40, offsetof(mavlink_serial_udb_extra_f16_t, sue_ID_DIY_DRONES_URL) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f16 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 sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot
- * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN];
-
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
-#else
-	mavlink_serial_udb_extra_f16_t packet;
-
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f16 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 sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot
- * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN];
-
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
-#else
-	mavlink_serial_udb_extra_f16_t packet;
-
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f16 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_udb_extra_f16 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16)
-{
-	return mavlink_msg_serial_udb_extra_f16_pack(system_id, component_id, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f16 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_udb_extra_f16 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16)
-{
-	return mavlink_msg_serial_udb_extra_f16_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL);
-}
-
-/**
- * @brief Send a serial_udb_extra_f16 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot
- * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f16_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F16 UNPACKING
-
-
-/**
- * @brief Get field sue_ID_LEAD_PILOT from serial_udb_extra_f16 message
- *
- * @return Serial UDB Extra Name of Expected Lead Pilot
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f16_get_sue_ID_LEAD_PILOT(const mavlink_message_t* msg, uint8_t *sue_ID_LEAD_PILOT)
-{
-	return _MAV_RETURN_uint8_t_array(msg, sue_ID_LEAD_PILOT, 40,  0);
-}
-
-/**
- * @brief Get field sue_ID_DIY_DRONES_URL from serial_udb_extra_f16 message
- *
- * @return Serial UDB Extra URL of Lead Pilot or Team
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f16_get_sue_ID_DIY_DRONES_URL(const mavlink_message_t* msg, uint8_t *sue_ID_DIY_DRONES_URL)
-{
-	return _MAV_RETURN_uint8_t_array(msg, sue_ID_DIY_DRONES_URL, 70,  40);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f16 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f16 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f16_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_serial_udb_extra_f16_get_sue_ID_LEAD_PILOT(msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT);
-	mavlink_msg_serial_udb_extra_f16_get_sue_ID_DIY_DRONES_URL(msg, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL);
-#else
-	memcpy(serial_udb_extra_f16, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
-#endif
-}
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
deleted file mode 100644
index 15ba68a34..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h
+++ /dev/null
@@ -1,771 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F2_A PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A 170
-
-typedef struct __mavlink_serial_udb_extra_f2_a_t
-{
- uint32_t sue_time; ///< Serial UDB Extra Time
- int32_t sue_latitude; ///< Serial UDB Extra Latitude
- int32_t sue_longitude; ///< Serial UDB Extra Longitude
- int32_t sue_altitude; ///< Serial UDB Extra Altitude
- uint16_t sue_waypoint_index; ///< Serial UDB Extra Waypoint Index
- int16_t sue_rmat0; ///< Serial UDB Extra Rmat 0
- int16_t sue_rmat1; ///< Serial UDB Extra Rmat 1
- int16_t sue_rmat2; ///< Serial UDB Extra Rmat 2
- int16_t sue_rmat3; ///< Serial UDB Extra Rmat 3
- int16_t sue_rmat4; ///< Serial UDB Extra Rmat 4
- int16_t sue_rmat5; ///< Serial UDB Extra Rmat 5
- int16_t sue_rmat6; ///< Serial UDB Extra Rmat 6
- int16_t sue_rmat7; ///< Serial UDB Extra Rmat 7
- int16_t sue_rmat8; ///< Serial UDB Extra Rmat 8
- uint16_t sue_cog; ///< Serial UDB Extra GPS Course Over Ground
- int16_t sue_sog; ///< Serial UDB Extra Speed Over Ground
- uint16_t sue_cpu_load; ///< Serial UDB Extra CPU Load
- int16_t sue_voltage_milis; ///< Serial UDB Extra Voltage in MilliVolts
- uint16_t sue_air_speed_3DIMU; ///< Serial UDB Extra 3D IMU Air Speed
- int16_t sue_estimated_wind_0; ///< Serial UDB Extra Estimated Wind 0
- int16_t sue_estimated_wind_1; ///< Serial UDB Extra Estimated Wind 1
- int16_t sue_estimated_wind_2; ///< Serial UDB Extra Estimated Wind 2
- int16_t sue_magFieldEarth0; ///< Serial UDB Extra Magnetic Field Earth 0 
- int16_t sue_magFieldEarth1; ///< Serial UDB Extra Magnetic Field Earth 1 
- int16_t sue_magFieldEarth2; ///< Serial UDB Extra Magnetic Field Earth 2 
- int16_t sue_svs; ///< Serial UDB Extra Number of Sattelites in View
- int16_t sue_hdop; ///< Serial UDB Extra GPS Horizontal Dilution of Precision
- uint8_t sue_status; ///< Serial UDB Extra Status
-} mavlink_serial_udb_extra_f2_a_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN 63
-#define MAVLINK_MSG_ID_170_LEN 63
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC 150
-#define MAVLINK_MSG_ID_170_CRC 150
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_A { \
-	"SERIAL_UDB_EXTRA_F2_A", \
-	28, \
-	{  { "sue_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_time) }, \
-         { "sue_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_latitude) }, \
-         { "sue_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_longitude) }, \
-         { "sue_altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_altitude) }, \
-         { "sue_waypoint_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_waypoint_index) }, \
-         { "sue_rmat0", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat0) }, \
-         { "sue_rmat1", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat1) }, \
-         { "sue_rmat2", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat2) }, \
-         { "sue_rmat3", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat3) }, \
-         { "sue_rmat4", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat4) }, \
-         { "sue_rmat5", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat5) }, \
-         { "sue_rmat6", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat6) }, \
-         { "sue_rmat7", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat7) }, \
-         { "sue_rmat8", NULL, MAVLINK_TYPE_INT16_T, 0, 34, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_rmat8) }, \
-         { "sue_cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_cog) }, \
-         { "sue_sog", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_sog) }, \
-         { "sue_cpu_load", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_cpu_load) }, \
-         { "sue_voltage_milis", NULL, MAVLINK_TYPE_INT16_T, 0, 42, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_voltage_milis) }, \
-         { "sue_air_speed_3DIMU", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_air_speed_3DIMU) }, \
-         { "sue_estimated_wind_0", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_0) }, \
-         { "sue_estimated_wind_1", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_1) }, \
-         { "sue_estimated_wind_2", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_estimated_wind_2) }, \
-         { "sue_magFieldEarth0", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth0) }, \
-         { "sue_magFieldEarth1", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth1) }, \
-         { "sue_magFieldEarth2", NULL, MAVLINK_TYPE_INT16_T, 0, 56, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_magFieldEarth2) }, \
-         { "sue_svs", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_svs) }, \
-         { "sue_hdop", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_hdop) }, \
-         { "sue_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_serial_udb_extra_f2_a_t, sue_status) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f2_a 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 sue_time Serial UDB Extra Time
- * @param sue_status Serial UDB Extra Status
- * @param sue_latitude Serial UDB Extra Latitude
- * @param sue_longitude Serial UDB Extra Longitude
- * @param sue_altitude Serial UDB Extra Altitude
- * @param sue_waypoint_index Serial UDB Extra Waypoint Index
- * @param sue_rmat0 Serial UDB Extra Rmat 0
- * @param sue_rmat1 Serial UDB Extra Rmat 1
- * @param sue_rmat2 Serial UDB Extra Rmat 2
- * @param sue_rmat3 Serial UDB Extra Rmat 3
- * @param sue_rmat4 Serial UDB Extra Rmat 4
- * @param sue_rmat5 Serial UDB Extra Rmat 5
- * @param sue_rmat6 Serial UDB Extra Rmat 6
- * @param sue_rmat7 Serial UDB Extra Rmat 7
- * @param sue_rmat8 Serial UDB Extra Rmat 8
- * @param sue_cog Serial UDB Extra GPS Course Over Ground
- * @param sue_sog Serial UDB Extra Speed Over Ground
- * @param sue_cpu_load Serial UDB Extra CPU Load
- * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts
- * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed
- * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0
- * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1
- * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2
- * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 
- * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 
- * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 
- * @param sue_svs Serial UDB Extra Number of Sattelites in View
- * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
-#else
-	mavlink_serial_udb_extra_f2_a_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f2_a 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 sue_time Serial UDB Extra Time
- * @param sue_status Serial UDB Extra Status
- * @param sue_latitude Serial UDB Extra Latitude
- * @param sue_longitude Serial UDB Extra Longitude
- * @param sue_altitude Serial UDB Extra Altitude
- * @param sue_waypoint_index Serial UDB Extra Waypoint Index
- * @param sue_rmat0 Serial UDB Extra Rmat 0
- * @param sue_rmat1 Serial UDB Extra Rmat 1
- * @param sue_rmat2 Serial UDB Extra Rmat 2
- * @param sue_rmat3 Serial UDB Extra Rmat 3
- * @param sue_rmat4 Serial UDB Extra Rmat 4
- * @param sue_rmat5 Serial UDB Extra Rmat 5
- * @param sue_rmat6 Serial UDB Extra Rmat 6
- * @param sue_rmat7 Serial UDB Extra Rmat 7
- * @param sue_rmat8 Serial UDB Extra Rmat 8
- * @param sue_cog Serial UDB Extra GPS Course Over Ground
- * @param sue_sog Serial UDB Extra Speed Over Ground
- * @param sue_cpu_load Serial UDB Extra CPU Load
- * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts
- * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed
- * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0
- * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1
- * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2
- * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 
- * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 
- * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 
- * @param sue_svs Serial UDB Extra Number of Sattelites in View
- * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
-#else
-	mavlink_serial_udb_extra_f2_a_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f2_a 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_udb_extra_f2_a C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a)
-{
-	return mavlink_msg_serial_udb_extra_f2_a_pack(system_id, component_id, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f2_a 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_udb_extra_f2_a C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a)
-{
-	return mavlink_msg_serial_udb_extra_f2_a_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop);
-}
-
-/**
- * @brief Send a serial_udb_extra_f2_a message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_time Serial UDB Extra Time
- * @param sue_status Serial UDB Extra Status
- * @param sue_latitude Serial UDB Extra Latitude
- * @param sue_longitude Serial UDB Extra Longitude
- * @param sue_altitude Serial UDB Extra Altitude
- * @param sue_waypoint_index Serial UDB Extra Waypoint Index
- * @param sue_rmat0 Serial UDB Extra Rmat 0
- * @param sue_rmat1 Serial UDB Extra Rmat 1
- * @param sue_rmat2 Serial UDB Extra Rmat 2
- * @param sue_rmat3 Serial UDB Extra Rmat 3
- * @param sue_rmat4 Serial UDB Extra Rmat 4
- * @param sue_rmat5 Serial UDB Extra Rmat 5
- * @param sue_rmat6 Serial UDB Extra Rmat 6
- * @param sue_rmat7 Serial UDB Extra Rmat 7
- * @param sue_rmat8 Serial UDB Extra Rmat 8
- * @param sue_cog Serial UDB Extra GPS Course Over Ground
- * @param sue_sog Serial UDB Extra Speed Over Ground
- * @param sue_cpu_load Serial UDB Extra CPU Load
- * @param sue_voltage_milis Serial UDB Extra Voltage in MilliVolts
- * @param sue_air_speed_3DIMU Serial UDB Extra 3D IMU Air Speed
- * @param sue_estimated_wind_0 Serial UDB Extra Estimated Wind 0
- * @param sue_estimated_wind_1 Serial UDB Extra Estimated Wind 1
- * @param sue_estimated_wind_2 Serial UDB Extra Estimated Wind 2
- * @param sue_magFieldEarth0 Serial UDB Extra Magnetic Field Earth 0 
- * @param sue_magFieldEarth1 Serial UDB Extra Magnetic Field Earth 1 
- * @param sue_magFieldEarth2 Serial UDB Extra Magnetic Field Earth 2 
- * @param sue_svs Serial UDB Extra Number of Sattelites in View
- * @param sue_hdop Serial UDB Extra GPS Horizontal Dilution of Precision
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f2_a_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F2_A UNPACKING
-
-
-/**
- * @brief Get field sue_time from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Time
- */
-static inline uint32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_time(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field sue_status from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Status
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f2_a_get_sue_status(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  62);
-}
-
-/**
- * @brief Get field sue_latitude from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Latitude
- */
-static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_latitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field sue_longitude from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Longitude
- */
-static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_longitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field sue_altitude from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Altitude
- */
-static inline int32_t mavlink_msg_serial_udb_extra_f2_a_get_sue_altitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  12);
-}
-
-/**
- * @brief Get field sue_waypoint_index from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Waypoint Index
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_waypoint_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field sue_rmat0 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 0
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat0(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  18);
-}
-
-/**
- * @brief Get field sue_rmat1 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 1
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Get field sue_rmat2 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 2
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  22);
-}
-
-/**
- * @brief Get field sue_rmat3 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 3
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  24);
-}
-
-/**
- * @brief Get field sue_rmat4 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 4
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  26);
-}
-
-/**
- * @brief Get field sue_rmat5 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 5
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  28);
-}
-
-/**
- * @brief Get field sue_rmat6 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 6
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  30);
-}
-
-/**
- * @brief Get field sue_rmat7 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 7
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat7(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  32);
-}
-
-/**
- * @brief Get field sue_rmat8 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Rmat 8
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  34);
-}
-
-/**
- * @brief Get field sue_cog from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra GPS Course Over Ground
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_cog(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  36);
-}
-
-/**
- * @brief Get field sue_sog from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Speed Over Ground
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_sog(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  38);
-}
-
-/**
- * @brief Get field sue_cpu_load from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra CPU Load
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_cpu_load(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  40);
-}
-
-/**
- * @brief Get field sue_voltage_milis from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Voltage in MilliVolts
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_voltage_milis(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  42);
-}
-
-/**
- * @brief Get field sue_air_speed_3DIMU from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra 3D IMU Air Speed
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_air_speed_3DIMU(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  44);
-}
-
-/**
- * @brief Get field sue_estimated_wind_0 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Estimated Wind 0
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_0(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  46);
-}
-
-/**
- * @brief Get field sue_estimated_wind_1 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Estimated Wind 1
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  48);
-}
-
-/**
- * @brief Get field sue_estimated_wind_2 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Estimated Wind 2
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  50);
-}
-
-/**
- * @brief Get field sue_magFieldEarth0 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Magnetic Field Earth 0 
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth0(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  52);
-}
-
-/**
- * @brief Get field sue_magFieldEarth1 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Magnetic Field Earth 1 
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  54);
-}
-
-/**
- * @brief Get field sue_magFieldEarth2 from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Magnetic Field Earth 2 
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  56);
-}
-
-/**
- * @brief Get field sue_svs from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra Number of Sattelites in View
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_svs(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  58);
-}
-
-/**
- * @brief Get field sue_hdop from serial_udb_extra_f2_a message
- *
- * @return Serial UDB Extra GPS Horizontal Dilution of Precision
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_a_get_sue_hdop(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  60);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f2_a message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f2_a C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f2_a_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f2_a->sue_time = mavlink_msg_serial_udb_extra_f2_a_get_sue_time(msg);
-	serial_udb_extra_f2_a->sue_latitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_latitude(msg);
-	serial_udb_extra_f2_a->sue_longitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_longitude(msg);
-	serial_udb_extra_f2_a->sue_altitude = mavlink_msg_serial_udb_extra_f2_a_get_sue_altitude(msg);
-	serial_udb_extra_f2_a->sue_waypoint_index = mavlink_msg_serial_udb_extra_f2_a_get_sue_waypoint_index(msg);
-	serial_udb_extra_f2_a->sue_rmat0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat0(msg);
-	serial_udb_extra_f2_a->sue_rmat1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat1(msg);
-	serial_udb_extra_f2_a->sue_rmat2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat2(msg);
-	serial_udb_extra_f2_a->sue_rmat3 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat3(msg);
-	serial_udb_extra_f2_a->sue_rmat4 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat4(msg);
-	serial_udb_extra_f2_a->sue_rmat5 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat5(msg);
-	serial_udb_extra_f2_a->sue_rmat6 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat6(msg);
-	serial_udb_extra_f2_a->sue_rmat7 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat7(msg);
-	serial_udb_extra_f2_a->sue_rmat8 = mavlink_msg_serial_udb_extra_f2_a_get_sue_rmat8(msg);
-	serial_udb_extra_f2_a->sue_cog = mavlink_msg_serial_udb_extra_f2_a_get_sue_cog(msg);
-	serial_udb_extra_f2_a->sue_sog = mavlink_msg_serial_udb_extra_f2_a_get_sue_sog(msg);
-	serial_udb_extra_f2_a->sue_cpu_load = mavlink_msg_serial_udb_extra_f2_a_get_sue_cpu_load(msg);
-	serial_udb_extra_f2_a->sue_voltage_milis = mavlink_msg_serial_udb_extra_f2_a_get_sue_voltage_milis(msg);
-	serial_udb_extra_f2_a->sue_air_speed_3DIMU = mavlink_msg_serial_udb_extra_f2_a_get_sue_air_speed_3DIMU(msg);
-	serial_udb_extra_f2_a->sue_estimated_wind_0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_0(msg);
-	serial_udb_extra_f2_a->sue_estimated_wind_1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_1(msg);
-	serial_udb_extra_f2_a->sue_estimated_wind_2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_estimated_wind_2(msg);
-	serial_udb_extra_f2_a->sue_magFieldEarth0 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth0(msg);
-	serial_udb_extra_f2_a->sue_magFieldEarth1 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth1(msg);
-	serial_udb_extra_f2_a->sue_magFieldEarth2 = mavlink_msg_serial_udb_extra_f2_a_get_sue_magFieldEarth2(msg);
-	serial_udb_extra_f2_a->sue_svs = mavlink_msg_serial_udb_extra_f2_a_get_sue_svs(msg);
-	serial_udb_extra_f2_a->sue_hdop = mavlink_msg_serial_udb_extra_f2_a_get_sue_hdop(msg);
-	serial_udb_extra_f2_a->sue_status = mavlink_msg_serial_udb_extra_f2_a_get_sue_status(msg);
-#else
-	memcpy(serial_udb_extra_f2_a, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
-#endif
-}
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
deleted file mode 100644
index 7cb8c87da..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h
+++ /dev/null
@@ -1,881 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F2_B PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B 171
-
-typedef struct __mavlink_serial_udb_extra_f2_b_t
-{
- uint32_t sue_time; ///< Serial UDB Extra Time
- uint32_t sue_flags; ///< Serial UDB Extra Status Flags
- int16_t sue_pwm_input_1; ///< Serial UDB Extra PWM Input Channel 1
- int16_t sue_pwm_input_2; ///< Serial UDB Extra PWM Input Channel 2
- int16_t sue_pwm_input_3; ///< Serial UDB Extra PWM Input Channel 3
- int16_t sue_pwm_input_4; ///< Serial UDB Extra PWM Input Channel 4
- int16_t sue_pwm_input_5; ///< Serial UDB Extra PWM Input Channel 5
- int16_t sue_pwm_input_6; ///< Serial UDB Extra PWM Input Channel 6
- int16_t sue_pwm_input_7; ///< Serial UDB Extra PWM Input Channel 7
- int16_t sue_pwm_input_8; ///< Serial UDB Extra PWM Input Channel 8
- int16_t sue_pwm_input_9; ///< Serial UDB Extra PWM Input Channel 9
- int16_t sue_pwm_input_10; ///< Serial UDB Extra PWM Input Channel 10
- int16_t sue_pwm_output_1; ///< Serial UDB Extra PWM Output Channel 1
- int16_t sue_pwm_output_2; ///< Serial UDB Extra PWM Output Channel 2
- int16_t sue_pwm_output_3; ///< Serial UDB Extra PWM Output Channel 3
- int16_t sue_pwm_output_4; ///< Serial UDB Extra PWM Output Channel 4
- int16_t sue_pwm_output_5; ///< Serial UDB Extra PWM Output Channel 5
- int16_t sue_pwm_output_6; ///< Serial UDB Extra PWM Output Channel 6
- int16_t sue_pwm_output_7; ///< Serial UDB Extra PWM Output Channel 7
- int16_t sue_pwm_output_8; ///< Serial UDB Extra PWM Output Channel 8
- int16_t sue_pwm_output_9; ///< Serial UDB Extra PWM Output Channel 9
- int16_t sue_pwm_output_10; ///< Serial UDB Extra PWM Output Channel 10
- int16_t sue_imu_location_x; ///< Serial UDB Extra IMU Location X
- int16_t sue_imu_location_y; ///< Serial UDB Extra IMU Location Y
- int16_t sue_imu_location_z; ///< Serial UDB Extra IMU Location Z
- int16_t sue_osc_fails; ///< Serial UDB Extra Oscillator Failure Count
- int16_t sue_imu_velocity_x; ///< Serial UDB Extra IMU Velocity X
- int16_t sue_imu_velocity_y; ///< Serial UDB Extra IMU Velocity Y
- int16_t sue_imu_velocity_z; ///< Serial UDB Extra IMU Velocity Z
- int16_t sue_waypoint_goal_x; ///< Serial UDB Extra Current Waypoint Goal X
- int16_t sue_waypoint_goal_y; ///< Serial UDB Extra Current Waypoint Goal Y
- int16_t sue_waypoint_goal_z; ///< Serial UDB Extra Current Waypoint Goal Z
- int16_t sue_memory_stack_free; ///< Serial UDB Extra Stack Memory Free
-} mavlink_serial_udb_extra_f2_b_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN 70
-#define MAVLINK_MSG_ID_171_LEN 70
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC 169
-#define MAVLINK_MSG_ID_171_CRC 169
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_B { \
-	"SERIAL_UDB_EXTRA_F2_B", \
-	33, \
-	{  { "sue_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_time) }, \
-         { "sue_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_flags) }, \
-         { "sue_pwm_input_1", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_1) }, \
-         { "sue_pwm_input_2", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_2) }, \
-         { "sue_pwm_input_3", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_3) }, \
-         { "sue_pwm_input_4", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_4) }, \
-         { "sue_pwm_input_5", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_5) }, \
-         { "sue_pwm_input_6", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_6) }, \
-         { "sue_pwm_input_7", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_7) }, \
-         { "sue_pwm_input_8", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_8) }, \
-         { "sue_pwm_input_9", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_9) }, \
-         { "sue_pwm_input_10", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_input_10) }, \
-         { "sue_pwm_output_1", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_1) }, \
-         { "sue_pwm_output_2", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_2) }, \
-         { "sue_pwm_output_3", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_3) }, \
-         { "sue_pwm_output_4", NULL, MAVLINK_TYPE_INT16_T, 0, 34, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_4) }, \
-         { "sue_pwm_output_5", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_5) }, \
-         { "sue_pwm_output_6", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_6) }, \
-         { "sue_pwm_output_7", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_7) }, \
-         { "sue_pwm_output_8", NULL, MAVLINK_TYPE_INT16_T, 0, 42, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_8) }, \
-         { "sue_pwm_output_9", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_9) }, \
-         { "sue_pwm_output_10", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_pwm_output_10) }, \
-         { "sue_imu_location_x", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_x) }, \
-         { "sue_imu_location_y", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_y) }, \
-         { "sue_imu_location_z", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_location_z) }, \
-         { "sue_osc_fails", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_osc_fails) }, \
-         { "sue_imu_velocity_x", NULL, MAVLINK_TYPE_INT16_T, 0, 56, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_x) }, \
-         { "sue_imu_velocity_y", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_y) }, \
-         { "sue_imu_velocity_z", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_imu_velocity_z) }, \
-         { "sue_waypoint_goal_x", NULL, MAVLINK_TYPE_INT16_T, 0, 62, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_x) }, \
-         { "sue_waypoint_goal_y", NULL, MAVLINK_TYPE_INT16_T, 0, 64, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_y) }, \
-         { "sue_waypoint_goal_z", NULL, MAVLINK_TYPE_INT16_T, 0, 66, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_waypoint_goal_z) }, \
-         { "sue_memory_stack_free", NULL, MAVLINK_TYPE_INT16_T, 0, 68, offsetof(mavlink_serial_udb_extra_f2_b_t, sue_memory_stack_free) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f2_b 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 sue_time Serial UDB Extra Time
- * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1
- * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2
- * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3
- * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4
- * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5
- * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6
- * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7
- * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8
- * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9
- * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10
- * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1
- * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2
- * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3
- * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4
- * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5
- * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6
- * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7
- * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8
- * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9
- * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10
- * @param sue_imu_location_x Serial UDB Extra IMU Location X
- * @param sue_imu_location_y Serial UDB Extra IMU Location Y
- * @param sue_imu_location_z Serial UDB Extra IMU Location Z
- * @param sue_flags Serial UDB Extra Status Flags
- * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count
- * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X
- * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y
- * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z
- * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X
- * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y
- * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z
- * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
-#else
-	mavlink_serial_udb_extra_f2_b_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f2_b 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 sue_time Serial UDB Extra Time
- * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1
- * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2
- * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3
- * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4
- * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5
- * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6
- * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7
- * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8
- * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9
- * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10
- * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1
- * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2
- * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3
- * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4
- * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5
- * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6
- * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7
- * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8
- * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9
- * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10
- * @param sue_imu_location_x Serial UDB Extra IMU Location X
- * @param sue_imu_location_y Serial UDB Extra IMU Location Y
- * @param sue_imu_location_z Serial UDB Extra IMU Location Z
- * @param sue_flags Serial UDB Extra Status Flags
- * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count
- * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X
- * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y
- * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z
- * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X
- * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y
- * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z
- * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
-#else
-	mavlink_serial_udb_extra_f2_b_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f2_b 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_udb_extra_f2_b C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b)
-{
-	return mavlink_msg_serial_udb_extra_f2_b_pack(system_id, component_id, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f2_b 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_udb_extra_f2_b C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b)
-{
-	return mavlink_msg_serial_udb_extra_f2_b_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free);
-}
-
-/**
- * @brief Send a serial_udb_extra_f2_b message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_time Serial UDB Extra Time
- * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1
- * @param sue_pwm_input_2 Serial UDB Extra PWM Input Channel 2
- * @param sue_pwm_input_3 Serial UDB Extra PWM Input Channel 3
- * @param sue_pwm_input_4 Serial UDB Extra PWM Input Channel 4
- * @param sue_pwm_input_5 Serial UDB Extra PWM Input Channel 5
- * @param sue_pwm_input_6 Serial UDB Extra PWM Input Channel 6
- * @param sue_pwm_input_7 Serial UDB Extra PWM Input Channel 7
- * @param sue_pwm_input_8 Serial UDB Extra PWM Input Channel 8
- * @param sue_pwm_input_9 Serial UDB Extra PWM Input Channel 9
- * @param sue_pwm_input_10 Serial UDB Extra PWM Input Channel 10
- * @param sue_pwm_output_1 Serial UDB Extra PWM Output Channel 1
- * @param sue_pwm_output_2 Serial UDB Extra PWM Output Channel 2
- * @param sue_pwm_output_3 Serial UDB Extra PWM Output Channel 3
- * @param sue_pwm_output_4 Serial UDB Extra PWM Output Channel 4
- * @param sue_pwm_output_5 Serial UDB Extra PWM Output Channel 5
- * @param sue_pwm_output_6 Serial UDB Extra PWM Output Channel 6
- * @param sue_pwm_output_7 Serial UDB Extra PWM Output Channel 7
- * @param sue_pwm_output_8 Serial UDB Extra PWM Output Channel 8
- * @param sue_pwm_output_9 Serial UDB Extra PWM Output Channel 9
- * @param sue_pwm_output_10 Serial UDB Extra PWM Output Channel 10
- * @param sue_imu_location_x Serial UDB Extra IMU Location X
- * @param sue_imu_location_y Serial UDB Extra IMU Location Y
- * @param sue_imu_location_z Serial UDB Extra IMU Location Z
- * @param sue_flags Serial UDB Extra Status Flags
- * @param sue_osc_fails Serial UDB Extra Oscillator Failure Count
- * @param sue_imu_velocity_x Serial UDB Extra IMU Velocity X
- * @param sue_imu_velocity_y Serial UDB Extra IMU Velocity Y
- * @param sue_imu_velocity_z Serial UDB Extra IMU Velocity Z
- * @param sue_waypoint_goal_x Serial UDB Extra Current Waypoint Goal X
- * @param sue_waypoint_goal_y Serial UDB Extra Current Waypoint Goal Y
- * @param sue_waypoint_goal_z Serial UDB Extra Current Waypoint Goal Z
- * @param sue_memory_stack_free Serial UDB Extra Stack Memory Free
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f2_b_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F2_B UNPACKING
-
-
-/**
- * @brief Get field sue_time from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra Time
- */
-static inline uint32_t mavlink_msg_serial_udb_extra_f2_b_get_sue_time(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field sue_pwm_input_1 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 1
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  8);
-}
-
-/**
- * @brief Get field sue_pwm_input_2 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 2
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  10);
-}
-
-/**
- * @brief Get field sue_pwm_input_3 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 3
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  12);
-}
-
-/**
- * @brief Get field sue_pwm_input_4 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 4
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  14);
-}
-
-/**
- * @brief Get field sue_pwm_input_5 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 5
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  16);
-}
-
-/**
- * @brief Get field sue_pwm_input_6 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 6
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  18);
-}
-
-/**
- * @brief Get field sue_pwm_input_7 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 7
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_7(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  20);
-}
-
-/**
- * @brief Get field sue_pwm_input_8 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 8
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  22);
-}
-
-/**
- * @brief Get field sue_pwm_input_9 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 9
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_9(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  24);
-}
-
-/**
- * @brief Get field sue_pwm_input_10 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Input Channel 10
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_10(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  26);
-}
-
-/**
- * @brief Get field sue_pwm_output_1 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 1
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  28);
-}
-
-/**
- * @brief Get field sue_pwm_output_2 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 2
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  30);
-}
-
-/**
- * @brief Get field sue_pwm_output_3 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 3
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  32);
-}
-
-/**
- * @brief Get field sue_pwm_output_4 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 4
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  34);
-}
-
-/**
- * @brief Get field sue_pwm_output_5 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 5
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  36);
-}
-
-/**
- * @brief Get field sue_pwm_output_6 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 6
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  38);
-}
-
-/**
- * @brief Get field sue_pwm_output_7 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 7
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_7(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  40);
-}
-
-/**
- * @brief Get field sue_pwm_output_8 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 8
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  42);
-}
-
-/**
- * @brief Get field sue_pwm_output_9 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 9
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_9(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  44);
-}
-
-/**
- * @brief Get field sue_pwm_output_10 from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra PWM Output Channel 10
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_10(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  46);
-}
-
-/**
- * @brief Get field sue_imu_location_x from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra IMU Location X
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  48);
-}
-
-/**
- * @brief Get field sue_imu_location_y from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra IMU Location Y
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  50);
-}
-
-/**
- * @brief Get field sue_imu_location_z from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra IMU Location Z
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  52);
-}
-
-/**
- * @brief Get field sue_flags from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra Status Flags
- */
-static inline uint32_t mavlink_msg_serial_udb_extra_f2_b_get_sue_flags(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  4);
-}
-
-/**
- * @brief Get field sue_osc_fails from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra Oscillator Failure Count
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_osc_fails(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  54);
-}
-
-/**
- * @brief Get field sue_imu_velocity_x from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra IMU Velocity X
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  56);
-}
-
-/**
- * @brief Get field sue_imu_velocity_y from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra IMU Velocity Y
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  58);
-}
-
-/**
- * @brief Get field sue_imu_velocity_z from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra IMU Velocity Z
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  60);
-}
-
-/**
- * @brief Get field sue_waypoint_goal_x from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra Current Waypoint Goal X
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  62);
-}
-
-/**
- * @brief Get field sue_waypoint_goal_y from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra Current Waypoint Goal Y
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  64);
-}
-
-/**
- * @brief Get field sue_waypoint_goal_z from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra Current Waypoint Goal Z
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  66);
-}
-
-/**
- * @brief Get field sue_memory_stack_free from serial_udb_extra_f2_b message
- *
- * @return Serial UDB Extra Stack Memory Free
- */
-static inline int16_t mavlink_msg_serial_udb_extra_f2_b_get_sue_memory_stack_free(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  68);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f2_b message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f2_b C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f2_b_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f2_b->sue_time = mavlink_msg_serial_udb_extra_f2_b_get_sue_time(msg);
-	serial_udb_extra_f2_b->sue_flags = mavlink_msg_serial_udb_extra_f2_b_get_sue_flags(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_1 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_1(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_2 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_2(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_3 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_3(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_4 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_4(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_5 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_5(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_6 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_6(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_7 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_7(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_8 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_8(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_9 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_9(msg);
-	serial_udb_extra_f2_b->sue_pwm_input_10 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_input_10(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_1 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_1(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_2 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_2(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_3 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_3(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_4 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_4(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_5 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_5(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_6 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_6(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_7 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_7(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_8 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_8(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_9 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_9(msg);
-	serial_udb_extra_f2_b->sue_pwm_output_10 = mavlink_msg_serial_udb_extra_f2_b_get_sue_pwm_output_10(msg);
-	serial_udb_extra_f2_b->sue_imu_location_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_x(msg);
-	serial_udb_extra_f2_b->sue_imu_location_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_y(msg);
-	serial_udb_extra_f2_b->sue_imu_location_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_location_z(msg);
-	serial_udb_extra_f2_b->sue_osc_fails = mavlink_msg_serial_udb_extra_f2_b_get_sue_osc_fails(msg);
-	serial_udb_extra_f2_b->sue_imu_velocity_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_x(msg);
-	serial_udb_extra_f2_b->sue_imu_velocity_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_y(msg);
-	serial_udb_extra_f2_b->sue_imu_velocity_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_imu_velocity_z(msg);
-	serial_udb_extra_f2_b->sue_waypoint_goal_x = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_x(msg);
-	serial_udb_extra_f2_b->sue_waypoint_goal_y = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_y(msg);
-	serial_udb_extra_f2_b->sue_waypoint_goal_z = mavlink_msg_serial_udb_extra_f2_b_get_sue_waypoint_goal_z(msg);
-	serial_udb_extra_f2_b->sue_memory_stack_free = mavlink_msg_serial_udb_extra_f2_b_get_sue_memory_stack_free(msg);
-#else
-	memcpy(serial_udb_extra_f2_b, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
-#endif
-}
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
deleted file mode 100644
index 77c616cc2..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h
+++ /dev/null
@@ -1,375 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F4 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4 172
-
-typedef struct __mavlink_serial_udb_extra_f4_t
-{
- uint8_t sue_ROLL_STABILIZATION_AILERONS; ///< Serial UDB Extra Roll Stabilization with Ailerons Enabled
- uint8_t sue_ROLL_STABILIZATION_RUDDER; ///< Serial UDB Extra Roll Stabilization with Rudder Enabled
- uint8_t sue_PITCH_STABILIZATION; ///< Serial UDB Extra Pitch Stabilization Enabled
- uint8_t sue_YAW_STABILIZATION_RUDDER; ///< Serial UDB Extra Yaw Stabilization using Rudder Enabled
- uint8_t sue_YAW_STABILIZATION_AILERON; ///< Serial UDB Extra Yaw Stabilization using Ailerons Enabled
- uint8_t sue_AILERON_NAVIGATION; ///< Serial UDB Extra Navigation with Ailerons Enabled
- uint8_t sue_RUDDER_NAVIGATION; ///< Serial UDB Extra Navigation with Rudder Enabled
- uint8_t sue_ALTITUDEHOLD_STABILIZED; ///< Serial UDB Extra Type of Alitude Hold when in Stabilized Mode
- uint8_t sue_ALTITUDEHOLD_WAYPOINT; ///< Serial UDB Extra Type of Alitude Hold when in Waypoint Mode
- uint8_t sue_RACING_MODE; ///< Serial UDB Extra Firmware racing mode enabled
-} mavlink_serial_udb_extra_f4_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN 10
-#define MAVLINK_MSG_ID_172_LEN 10
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC 191
-#define MAVLINK_MSG_ID_172_CRC 191
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F4 { \
-	"SERIAL_UDB_EXTRA_F4", \
-	10, \
-	{  { "sue_ROLL_STABILIZATION_AILERONS", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_serial_udb_extra_f4_t, sue_ROLL_STABILIZATION_AILERONS) }, \
-         { "sue_ROLL_STABILIZATION_RUDDER", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_serial_udb_extra_f4_t, sue_ROLL_STABILIZATION_RUDDER) }, \
-         { "sue_PITCH_STABILIZATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_serial_udb_extra_f4_t, sue_PITCH_STABILIZATION) }, \
-         { "sue_YAW_STABILIZATION_RUDDER", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_serial_udb_extra_f4_t, sue_YAW_STABILIZATION_RUDDER) }, \
-         { "sue_YAW_STABILIZATION_AILERON", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_serial_udb_extra_f4_t, sue_YAW_STABILIZATION_AILERON) }, \
-         { "sue_AILERON_NAVIGATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_serial_udb_extra_f4_t, sue_AILERON_NAVIGATION) }, \
-         { "sue_RUDDER_NAVIGATION", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_udb_extra_f4_t, sue_RUDDER_NAVIGATION) }, \
-         { "sue_ALTITUDEHOLD_STABILIZED", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_udb_extra_f4_t, sue_ALTITUDEHOLD_STABILIZED) }, \
-         { "sue_ALTITUDEHOLD_WAYPOINT", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_udb_extra_f4_t, sue_ALTITUDEHOLD_WAYPOINT) }, \
-         { "sue_RACING_MODE", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_serial_udb_extra_f4_t, sue_RACING_MODE) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f4 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 sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled
- * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled
- * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled
- * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled
- * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled
- * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled
- * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled
- * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode
- * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode
- * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
-#else
-	mavlink_serial_udb_extra_f4_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f4 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 sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled
- * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled
- * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled
- * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled
- * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled
- * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled
- * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled
- * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode
- * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode
- * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
-#else
-	mavlink_serial_udb_extra_f4_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f4 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_udb_extra_f4 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4)
-{
-	return mavlink_msg_serial_udb_extra_f4_pack(system_id, component_id, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f4 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_udb_extra_f4 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4)
-{
-	return mavlink_msg_serial_udb_extra_f4_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE);
-}
-
-/**
- * @brief Send a serial_udb_extra_f4 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled
- * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled
- * @param sue_PITCH_STABILIZATION Serial UDB Extra Pitch Stabilization Enabled
- * @param sue_YAW_STABILIZATION_RUDDER Serial UDB Extra Yaw Stabilization using Rudder Enabled
- * @param sue_YAW_STABILIZATION_AILERON Serial UDB Extra Yaw Stabilization using Ailerons Enabled
- * @param sue_AILERON_NAVIGATION Serial UDB Extra Navigation with Ailerons Enabled
- * @param sue_RUDDER_NAVIGATION Serial UDB Extra Navigation with Rudder Enabled
- * @param sue_ALTITUDEHOLD_STABILIZED Serial UDB Extra Type of Alitude Hold when in Stabilized Mode
- * @param sue_ALTITUDEHOLD_WAYPOINT Serial UDB Extra Type of Alitude Hold when in Waypoint Mode
- * @param sue_RACING_MODE Serial UDB Extra Firmware racing mode enabled
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f4_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F4 UNPACKING
-
-
-/**
- * @brief Get field sue_ROLL_STABILIZATION_AILERONS from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Roll Stabilization with Ailerons Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_AILERONS(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field sue_ROLL_STABILIZATION_RUDDER from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Roll Stabilization with Rudder Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_RUDDER(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field sue_PITCH_STABILIZATION from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Pitch Stabilization Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_PITCH_STABILIZATION(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field sue_YAW_STABILIZATION_RUDDER from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Yaw Stabilization using Rudder Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_RUDDER(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field sue_YAW_STABILIZATION_AILERON from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Yaw Stabilization using Ailerons Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_AILERON(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field sue_AILERON_NAVIGATION from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Navigation with Ailerons Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_AILERON_NAVIGATION(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field sue_RUDDER_NAVIGATION from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Navigation with Rudder Enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_RUDDER_NAVIGATION(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field sue_ALTITUDEHOLD_STABILIZED from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Type of Alitude Hold when in Stabilized Mode
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_STABILIZED(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field sue_ALTITUDEHOLD_WAYPOINT from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Type of Alitude Hold when in Waypoint Mode
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_WAYPOINT(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field sue_RACING_MODE from serial_udb_extra_f4 message
- *
- * @return Serial UDB Extra Firmware racing mode enabled
- */
-static inline uint8_t mavlink_msg_serial_udb_extra_f4_get_sue_RACING_MODE(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f4 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f4 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f4_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS = mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_AILERONS(msg);
-	serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER = mavlink_msg_serial_udb_extra_f4_get_sue_ROLL_STABILIZATION_RUDDER(msg);
-	serial_udb_extra_f4->sue_PITCH_STABILIZATION = mavlink_msg_serial_udb_extra_f4_get_sue_PITCH_STABILIZATION(msg);
-	serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER = mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_RUDDER(msg);
-	serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON = mavlink_msg_serial_udb_extra_f4_get_sue_YAW_STABILIZATION_AILERON(msg);
-	serial_udb_extra_f4->sue_AILERON_NAVIGATION = mavlink_msg_serial_udb_extra_f4_get_sue_AILERON_NAVIGATION(msg);
-	serial_udb_extra_f4->sue_RUDDER_NAVIGATION = mavlink_msg_serial_udb_extra_f4_get_sue_RUDDER_NAVIGATION(msg);
-	serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED = mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_STABILIZED(msg);
-	serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT = mavlink_msg_serial_udb_extra_f4_get_sue_ALTITUDEHOLD_WAYPOINT(msg);
-	serial_udb_extra_f4->sue_RACING_MODE = mavlink_msg_serial_udb_extra_f4_get_sue_RACING_MODE(msg);
-#else
-	memcpy(serial_udb_extra_f4, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
-#endif
-}
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
deleted file mode 100644
index e115a9b44..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F5 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5 173
-
-typedef struct __mavlink_serial_udb_extra_f5_t
-{
- float sue_YAWKP_AILERON; ///< Serial UDB YAWKP_AILERON Gain for Proporional control of navigation
- float sue_YAWKD_AILERON; ///< Serial UDB YAWKD_AILERON Gain for Rate control of navigation
- float sue_ROLLKP; ///< Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization
- float sue_ROLLKD; ///< Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization
- float sue_YAW_STABILIZATION_AILERON; ///< YAW_STABILIZATION_AILERON Proportional control
- float sue_AILERON_BOOST; ///< Gain For Boosting Manual Aileron control When Plane Stabilized
-} mavlink_serial_udb_extra_f5_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN 24
-#define MAVLINK_MSG_ID_173_LEN 24
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC 121
-#define MAVLINK_MSG_ID_173_CRC 121
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F5 { \
-	"SERIAL_UDB_EXTRA_F5", \
-	6, \
-	{  { "sue_YAWKP_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAWKP_AILERON) }, \
-         { "sue_YAWKD_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAWKD_AILERON) }, \
-         { "sue_ROLLKP", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f5_t, sue_ROLLKP) }, \
-         { "sue_ROLLKD", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f5_t, sue_ROLLKD) }, \
-         { "sue_YAW_STABILIZATION_AILERON", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f5_t, sue_YAW_STABILIZATION_AILERON) }, \
-         { "sue_AILERON_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f5_t, sue_AILERON_BOOST) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f5 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 sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation
- * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation
- * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization
- * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization
- * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control
- * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
-#else
-	mavlink_serial_udb_extra_f5_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f5 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 sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation
- * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation
- * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization
- * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization
- * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control
- * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
-#else
-	mavlink_serial_udb_extra_f5_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f5 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_udb_extra_f5 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5)
-{
-	return mavlink_msg_serial_udb_extra_f5_pack(system_id, component_id, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f5 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_udb_extra_f5 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5)
-{
-	return mavlink_msg_serial_udb_extra_f5_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST);
-}
-
-/**
- * @brief Send a serial_udb_extra_f5 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation
- * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation
- * @param sue_ROLLKP Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization
- * @param sue_ROLLKD Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization
- * @param sue_YAW_STABILIZATION_AILERON YAW_STABILIZATION_AILERON Proportional control
- * @param sue_AILERON_BOOST Gain For Boosting Manual Aileron control When Plane Stabilized
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f5_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F5 UNPACKING
-
-
-/**
- * @brief Get field sue_YAWKP_AILERON from serial_udb_extra_f5 message
- *
- * @return Serial UDB YAWKP_AILERON Gain for Proporional control of navigation
- */
-static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAWKP_AILERON(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field sue_YAWKD_AILERON from serial_udb_extra_f5 message
- *
- * @return Serial UDB YAWKD_AILERON Gain for Rate control of navigation
- */
-static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAWKD_AILERON(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field sue_ROLLKP from serial_udb_extra_f5 message
- *
- * @return Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization
- */
-static inline float mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKP(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field sue_ROLLKD from serial_udb_extra_f5 message
- *
- * @return Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization
- */
-static inline float mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKD(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field sue_YAW_STABILIZATION_AILERON from serial_udb_extra_f5 message
- *
- * @return YAW_STABILIZATION_AILERON Proportional control
- */
-static inline float mavlink_msg_serial_udb_extra_f5_get_sue_YAW_STABILIZATION_AILERON(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field sue_AILERON_BOOST from serial_udb_extra_f5 message
- *
- * @return Gain For Boosting Manual Aileron control When Plane Stabilized
- */
-static inline float mavlink_msg_serial_udb_extra_f5_get_sue_AILERON_BOOST(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f5 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f5 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f5_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f5->sue_YAWKP_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAWKP_AILERON(msg);
-	serial_udb_extra_f5->sue_YAWKD_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAWKD_AILERON(msg);
-	serial_udb_extra_f5->sue_ROLLKP = mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKP(msg);
-	serial_udb_extra_f5->sue_ROLLKD = mavlink_msg_serial_udb_extra_f5_get_sue_ROLLKD(msg);
-	serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON = mavlink_msg_serial_udb_extra_f5_get_sue_YAW_STABILIZATION_AILERON(msg);
-	serial_udb_extra_f5->sue_AILERON_BOOST = mavlink_msg_serial_udb_extra_f5_get_sue_AILERON_BOOST(msg);
-#else
-	memcpy(serial_udb_extra_f5, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
-#endif
-}
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
deleted file mode 100644
index 656103d09..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F6 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6 174
-
-typedef struct __mavlink_serial_udb_extra_f6_t
-{
- float sue_PITCHGAIN; ///< Serial UDB Extra PITCHGAIN Proportional Control
- float sue_PITCHKD; ///< Serial UDB Extra Pitch Rate Control
- float sue_RUDDER_ELEV_MIX; ///< Serial UDB Extra Rudder to Elevator Mix
- float sue_ROLL_ELEV_MIX; ///< Serial UDB Extra Roll to Elevator Mix
- float sue_ELEVATOR_BOOST; ///< Gain For Boosting Manual Elevator control When Plane Stabilized
-} mavlink_serial_udb_extra_f6_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN 20
-#define MAVLINK_MSG_ID_174_LEN 20
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC 54
-#define MAVLINK_MSG_ID_174_CRC 54
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F6 { \
-	"SERIAL_UDB_EXTRA_F6", \
-	5, \
-	{  { "sue_PITCHGAIN", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f6_t, sue_PITCHGAIN) }, \
-         { "sue_PITCHKD", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f6_t, sue_PITCHKD) }, \
-         { "sue_RUDDER_ELEV_MIX", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f6_t, sue_RUDDER_ELEV_MIX) }, \
-         { "sue_ROLL_ELEV_MIX", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f6_t, sue_ROLL_ELEV_MIX) }, \
-         { "sue_ELEVATOR_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f6_t, sue_ELEVATOR_BOOST) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f6 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 sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control
- * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control
- * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix
- * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix
- * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
-#else
-	mavlink_serial_udb_extra_f6_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f6 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 sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control
- * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control
- * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix
- * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix
- * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
-#else
-	mavlink_serial_udb_extra_f6_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f6 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_udb_extra_f6 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6)
-{
-	return mavlink_msg_serial_udb_extra_f6_pack(system_id, component_id, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f6 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_udb_extra_f6 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6)
-{
-	return mavlink_msg_serial_udb_extra_f6_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST);
-}
-
-/**
- * @brief Send a serial_udb_extra_f6 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control
- * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control
- * @param sue_RUDDER_ELEV_MIX Serial UDB Extra Rudder to Elevator Mix
- * @param sue_ROLL_ELEV_MIX Serial UDB Extra Roll to Elevator Mix
- * @param sue_ELEVATOR_BOOST Gain For Boosting Manual Elevator control When Plane Stabilized
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f6_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F6 UNPACKING
-
-
-/**
- * @brief Get field sue_PITCHGAIN from serial_udb_extra_f6 message
- *
- * @return Serial UDB Extra PITCHGAIN Proportional Control
- */
-static inline float mavlink_msg_serial_udb_extra_f6_get_sue_PITCHGAIN(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field sue_PITCHKD from serial_udb_extra_f6 message
- *
- * @return Serial UDB Extra Pitch Rate Control
- */
-static inline float mavlink_msg_serial_udb_extra_f6_get_sue_PITCHKD(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field sue_RUDDER_ELEV_MIX from serial_udb_extra_f6 message
- *
- * @return Serial UDB Extra Rudder to Elevator Mix
- */
-static inline float mavlink_msg_serial_udb_extra_f6_get_sue_RUDDER_ELEV_MIX(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field sue_ROLL_ELEV_MIX from serial_udb_extra_f6 message
- *
- * @return Serial UDB Extra Roll to Elevator Mix
- */
-static inline float mavlink_msg_serial_udb_extra_f6_get_sue_ROLL_ELEV_MIX(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field sue_ELEVATOR_BOOST from serial_udb_extra_f6 message
- *
- * @return Gain For Boosting Manual Elevator control When Plane Stabilized
- */
-static inline float mavlink_msg_serial_udb_extra_f6_get_sue_ELEVATOR_BOOST(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f6 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f6 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f6_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f6->sue_PITCHGAIN = mavlink_msg_serial_udb_extra_f6_get_sue_PITCHGAIN(msg);
-	serial_udb_extra_f6->sue_PITCHKD = mavlink_msg_serial_udb_extra_f6_get_sue_PITCHKD(msg);
-	serial_udb_extra_f6->sue_RUDDER_ELEV_MIX = mavlink_msg_serial_udb_extra_f6_get_sue_RUDDER_ELEV_MIX(msg);
-	serial_udb_extra_f6->sue_ROLL_ELEV_MIX = mavlink_msg_serial_udb_extra_f6_get_sue_ROLL_ELEV_MIX(msg);
-	serial_udb_extra_f6->sue_ELEVATOR_BOOST = mavlink_msg_serial_udb_extra_f6_get_sue_ELEVATOR_BOOST(msg);
-#else
-	memcpy(serial_udb_extra_f6, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
-#endif
-}
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
deleted file mode 100644
index 51c98e6b7..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F7 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7 175
-
-typedef struct __mavlink_serial_udb_extra_f7_t
-{
- float sue_YAWKP_RUDDER; ///< Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation
- float sue_YAWKD_RUDDER; ///< Serial UDB YAWKD_RUDDER Gain for Rate control of navigation
- float sue_ROLLKP_RUDDER; ///< Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization
- float sue_ROLLKD_RUDDER; ///< Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization
- float sue_RUDDER_BOOST; ///< SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized
- float sue_RTL_PITCH_DOWN; ///< Serial UDB Extra Return To Landing - Angle to Pitch Plane Down
-} mavlink_serial_udb_extra_f7_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN 24
-#define MAVLINK_MSG_ID_175_LEN 24
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC 171
-#define MAVLINK_MSG_ID_175_CRC 171
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F7 { \
-	"SERIAL_UDB_EXTRA_F7", \
-	6, \
-	{  { "sue_YAWKP_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f7_t, sue_YAWKP_RUDDER) }, \
-         { "sue_YAWKD_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f7_t, sue_YAWKD_RUDDER) }, \
-         { "sue_ROLLKP_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f7_t, sue_ROLLKP_RUDDER) }, \
-         { "sue_ROLLKD_RUDDER", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f7_t, sue_ROLLKD_RUDDER) }, \
-         { "sue_RUDDER_BOOST", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f7_t, sue_RUDDER_BOOST) }, \
-         { "sue_RTL_PITCH_DOWN", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f7_t, sue_RTL_PITCH_DOWN) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f7 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 sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation
- * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation
- * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization
- * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization
- * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized
- * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
-#else
-	mavlink_serial_udb_extra_f7_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f7 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 sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation
- * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation
- * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization
- * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization
- * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized
- * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
-#else
-	mavlink_serial_udb_extra_f7_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f7 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_udb_extra_f7 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7)
-{
-	return mavlink_msg_serial_udb_extra_f7_pack(system_id, component_id, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f7 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_udb_extra_f7 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7)
-{
-	return mavlink_msg_serial_udb_extra_f7_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN);
-}
-
-/**
- * @brief Send a serial_udb_extra_f7 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation
- * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation
- * @param sue_ROLLKP_RUDDER Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization
- * @param sue_ROLLKD_RUDDER Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization
- * @param sue_RUDDER_BOOST SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized
- * @param sue_RTL_PITCH_DOWN Serial UDB Extra Return To Landing - Angle to Pitch Plane Down
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f7_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F7 UNPACKING
-
-
-/**
- * @brief Get field sue_YAWKP_RUDDER from serial_udb_extra_f7 message
- *
- * @return Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation
- */
-static inline float mavlink_msg_serial_udb_extra_f7_get_sue_YAWKP_RUDDER(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field sue_YAWKD_RUDDER from serial_udb_extra_f7 message
- *
- * @return Serial UDB YAWKD_RUDDER Gain for Rate control of navigation
- */
-static inline float mavlink_msg_serial_udb_extra_f7_get_sue_YAWKD_RUDDER(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field sue_ROLLKP_RUDDER from serial_udb_extra_f7 message
- *
- * @return Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization
- */
-static inline float mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKP_RUDDER(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field sue_ROLLKD_RUDDER from serial_udb_extra_f7 message
- *
- * @return Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization
- */
-static inline float mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKD_RUDDER(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field sue_RUDDER_BOOST from serial_udb_extra_f7 message
- *
- * @return SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized
- */
-static inline float mavlink_msg_serial_udb_extra_f7_get_sue_RUDDER_BOOST(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field sue_RTL_PITCH_DOWN from serial_udb_extra_f7 message
- *
- * @return Serial UDB Extra Return To Landing - Angle to Pitch Plane Down
- */
-static inline float mavlink_msg_serial_udb_extra_f7_get_sue_RTL_PITCH_DOWN(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f7 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f7 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f7_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f7->sue_YAWKP_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_YAWKP_RUDDER(msg);
-	serial_udb_extra_f7->sue_YAWKD_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_YAWKD_RUDDER(msg);
-	serial_udb_extra_f7->sue_ROLLKP_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKP_RUDDER(msg);
-	serial_udb_extra_f7->sue_ROLLKD_RUDDER = mavlink_msg_serial_udb_extra_f7_get_sue_ROLLKD_RUDDER(msg);
-	serial_udb_extra_f7->sue_RUDDER_BOOST = mavlink_msg_serial_udb_extra_f7_get_sue_RUDDER_BOOST(msg);
-	serial_udb_extra_f7->sue_RTL_PITCH_DOWN = mavlink_msg_serial_udb_extra_f7_get_sue_RTL_PITCH_DOWN(msg);
-#else
-	memcpy(serial_udb_extra_f7, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
-#endif
-}
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
deleted file mode 100644
index 8557a95e2..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE SERIAL_UDB_EXTRA_F8 PACKING
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8 176
-
-typedef struct __mavlink_serial_udb_extra_f8_t
-{
- float sue_HEIGHT_TARGET_MAX; ///< Serial UDB Extra HEIGHT_TARGET_MAX
- float sue_HEIGHT_TARGET_MIN; ///< Serial UDB Extra HEIGHT_TARGET_MIN
- float sue_ALT_HOLD_THROTTLE_MIN; ///< Serial UDB Extra ALT_HOLD_THROTTLE_MIN
- float sue_ALT_HOLD_THROTTLE_MAX; ///< Serial UDB Extra ALT_HOLD_THROTTLE_MAX
- float sue_ALT_HOLD_PITCH_MIN; ///< Serial UDB Extra ALT_HOLD_PITCH_MIN
- float sue_ALT_HOLD_PITCH_MAX; ///< Serial UDB Extra ALT_HOLD_PITCH_MAX
- float sue_ALT_HOLD_PITCH_HIGH; ///< Serial UDB Extra ALT_HOLD_PITCH_HIGH
-} mavlink_serial_udb_extra_f8_t;
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN 28
-#define MAVLINK_MSG_ID_176_LEN 28
-
-#define MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC 142
-#define MAVLINK_MSG_ID_176_CRC 142
-
-
-
-#define MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F8 { \
-	"SERIAL_UDB_EXTRA_F8", \
-	7, \
-	{  { "sue_HEIGHT_TARGET_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_serial_udb_extra_f8_t, sue_HEIGHT_TARGET_MAX) }, \
-         { "sue_HEIGHT_TARGET_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_serial_udb_extra_f8_t, sue_HEIGHT_TARGET_MIN) }, \
-         { "sue_ALT_HOLD_THROTTLE_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_THROTTLE_MIN) }, \
-         { "sue_ALT_HOLD_THROTTLE_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_THROTTLE_MAX) }, \
-         { "sue_ALT_HOLD_PITCH_MIN", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_MIN) }, \
-         { "sue_ALT_HOLD_PITCH_MAX", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_MAX) }, \
-         { "sue_ALT_HOLD_PITCH_HIGH", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_serial_udb_extra_f8_t, sue_ALT_HOLD_PITCH_HIGH) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a serial_udb_extra_f8 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 sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX
- * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN
- * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN
- * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX
- * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN
- * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX
- * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
-#else
-	mavlink_serial_udb_extra_f8_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
-#endif
-}
-
-/**
- * @brief Pack a serial_udb_extra_f8 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 sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX
- * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN
- * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN
- * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX
- * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN
- * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX
- * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
-#else
-	mavlink_serial_udb_extra_f8_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
-#endif
-}
-
-/**
- * @brief Encode a serial_udb_extra_f8 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_udb_extra_f8 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8)
-{
-	return mavlink_msg_serial_udb_extra_f8_pack(system_id, component_id, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH);
-}
-
-/**
- * @brief Encode a serial_udb_extra_f8 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_udb_extra_f8 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8)
-{
-	return mavlink_msg_serial_udb_extra_f8_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH);
-}
-
-/**
- * @brief Send a serial_udb_extra_f8 message
- * @param chan MAVLink channel to send the message
- *
- * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX
- * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN
- * @param sue_ALT_HOLD_THROTTLE_MIN Serial UDB Extra ALT_HOLD_THROTTLE_MIN
- * @param sue_ALT_HOLD_THROTTLE_MAX Serial UDB Extra ALT_HOLD_THROTTLE_MAX
- * @param sue_ALT_HOLD_PITCH_MIN Serial UDB Extra ALT_HOLD_PITCH_MIN
- * @param sue_ALT_HOLD_PITCH_MAX Serial UDB Extra ALT_HOLD_PITCH_MAX
- * @param sue_ALT_HOLD_PITCH_HIGH Serial UDB Extra ALT_HOLD_PITCH_HIGH
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_serial_udb_extra_f8_send(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[MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN];
-	_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;
-	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
-
-// MESSAGE SERIAL_UDB_EXTRA_F8 UNPACKING
-
-
-/**
- * @brief Get field sue_HEIGHT_TARGET_MAX from serial_udb_extra_f8 message
- *
- * @return Serial UDB Extra HEIGHT_TARGET_MAX
- */
-static inline float mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MAX(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field sue_HEIGHT_TARGET_MIN from serial_udb_extra_f8 message
- *
- * @return Serial UDB Extra HEIGHT_TARGET_MIN
- */
-static inline float mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MIN(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field sue_ALT_HOLD_THROTTLE_MIN from serial_udb_extra_f8 message
- *
- * @return Serial UDB Extra ALT_HOLD_THROTTLE_MIN
- */
-static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MIN(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field sue_ALT_HOLD_THROTTLE_MAX from serial_udb_extra_f8 message
- *
- * @return Serial UDB Extra ALT_HOLD_THROTTLE_MAX
- */
-static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MAX(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field sue_ALT_HOLD_PITCH_MIN from serial_udb_extra_f8 message
- *
- * @return Serial UDB Extra ALT_HOLD_PITCH_MIN
- */
-static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MIN(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field sue_ALT_HOLD_PITCH_MAX from serial_udb_extra_f8 message
- *
- * @return Serial UDB Extra ALT_HOLD_PITCH_MAX
- */
-static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MAX(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field sue_ALT_HOLD_PITCH_HIGH from serial_udb_extra_f8 message
- *
- * @return Serial UDB Extra ALT_HOLD_PITCH_HIGH
- */
-static inline float mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_HIGH(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a serial_udb_extra_f8 message into a struct
- *
- * @param msg The message to decode
- * @param serial_udb_extra_f8 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_serial_udb_extra_f8_decode(const mavlink_message_t* msg, mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MAX(msg);
-	serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_HEIGHT_TARGET_MIN(msg);
-	serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MIN(msg);
-	serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_THROTTLE_MAX(msg);
-	serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MIN(msg);
-	serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_MAX(msg);
-	serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH = mavlink_msg_serial_udb_extra_f8_get_sue_ALT_HOLD_PITCH_HIGH(msg);
-#else
-	memcpy(serial_udb_extra_f8, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h b/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h
deleted file mode 100644
index 6856839e9..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h
+++ /dev/null
@@ -1,1240 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from matrixpilot.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef MATRIXPILOT_TESTSUITE_H
-#define MATRIXPILOT_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg);
-static void mavlink_test_matrixpilot(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_common(system_id, component_id, last_msg);
-	mavlink_test_matrixpilot(system_id, component_id, last_msg);
-}
-#endif
-
-#include "../common/testsuite.h"
-
-
-static void mavlink_test_flexifunction_set(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_flexifunction_set_t packet_in = {
-		5,
-	}72,
-	};
-	mavlink_flexifunction_set_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_set_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_flexifunction_set_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_set_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_flexifunction_set_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component );
-	mavlink_msg_flexifunction_set_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_flexifunction_set_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_set_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component );
-	mavlink_msg_flexifunction_set_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_flexifunction_read_req(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_flexifunction_read_req_t packet_in = {
-		17235,
-	}17339,
-	}17,
-	}84,
-	};
-	mavlink_flexifunction_read_req_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.read_req_type = packet_in.read_req_type;
-        	packet1.data_index = packet_in.data_index;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_read_req_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_flexifunction_read_req_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_read_req_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.read_req_type , packet1.data_index );
-	mavlink_msg_flexifunction_read_req_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_read_req_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.read_req_type , packet1.data_index );
-	mavlink_msg_flexifunction_read_req_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_flexifunction_read_req_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_read_req_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.read_req_type , packet1.data_index );
-	mavlink_msg_flexifunction_read_req_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_flexifunction_buffer_function(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_flexifunction_buffer_function_t packet_in = {
-		17235,
-	}17339,
-	}17443,
-	}17547,
-	}29,
-	}96,
-	}{ 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210 },
-	};
-	mavlink_flexifunction_buffer_function_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.func_index = packet_in.func_index;
-        	packet1.func_count = packet_in.func_count;
-        	packet1.data_address = packet_in.data_address;
-        	packet1.data_size = packet_in.data_size;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(int8_t)*48);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_buffer_function_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_flexifunction_buffer_function_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_buffer_function_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.func_index , packet1.func_count , packet1.data_address , packet1.data_size , packet1.data );
-	mavlink_msg_flexifunction_buffer_function_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_buffer_function_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.func_index , packet1.func_count , packet1.data_address , packet1.data_size , packet1.data );
-	mavlink_msg_flexifunction_buffer_function_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_flexifunction_buffer_function_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_buffer_function_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.func_index , packet1.func_count , packet1.data_address , packet1.data_size , packet1.data );
-	mavlink_msg_flexifunction_buffer_function_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_flexifunction_buffer_function_ack(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_flexifunction_buffer_function_ack_t packet_in = {
-		17235,
-	}17339,
-	}17,
-	}84,
-	};
-	mavlink_flexifunction_buffer_function_ack_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.func_index = packet_in.func_index;
-        	packet1.result = packet_in.result;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_buffer_function_ack_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_flexifunction_buffer_function_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_buffer_function_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.func_index , packet1.result );
-	mavlink_msg_flexifunction_buffer_function_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_buffer_function_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.func_index , packet1.result );
-	mavlink_msg_flexifunction_buffer_function_ack_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_flexifunction_buffer_function_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_buffer_function_ack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.func_index , packet1.result );
-	mavlink_msg_flexifunction_buffer_function_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_flexifunction_directory(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_flexifunction_directory_t packet_in = {
-		5,
-	}72,
-	}139,
-	}206,
-	}17,
-	}{ 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 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 },
-	};
-	mavlink_flexifunction_directory_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.directory_type = packet_in.directory_type;
-        	packet1.start_index = packet_in.start_index;
-        	packet1.count = packet_in.count;
-        
-        	mav_array_memcpy(packet1.directory_data, packet_in.directory_data, sizeof(int8_t)*48);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_directory_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_flexifunction_directory_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_directory_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.directory_type , packet1.start_index , packet1.count , packet1.directory_data );
-	mavlink_msg_flexifunction_directory_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_directory_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.directory_type , packet1.start_index , packet1.count , packet1.directory_data );
-	mavlink_msg_flexifunction_directory_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_flexifunction_directory_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_directory_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.directory_type , packet1.start_index , packet1.count , packet1.directory_data );
-	mavlink_msg_flexifunction_directory_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_flexifunction_directory_ack(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_flexifunction_directory_ack_t packet_in = {
-		17235,
-	}139,
-	}206,
-	}17,
-	}84,
-	}151,
-	};
-	mavlink_flexifunction_directory_ack_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.result = packet_in.result;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.directory_type = packet_in.directory_type;
-        	packet1.start_index = packet_in.start_index;
-        	packet1.count = packet_in.count;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_directory_ack_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_flexifunction_directory_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_directory_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.directory_type , packet1.start_index , packet1.count , packet1.result );
-	mavlink_msg_flexifunction_directory_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_directory_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.directory_type , packet1.start_index , packet1.count , packet1.result );
-	mavlink_msg_flexifunction_directory_ack_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_flexifunction_directory_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_directory_ack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.directory_type , packet1.start_index , packet1.count , packet1.result );
-	mavlink_msg_flexifunction_directory_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_flexifunction_command(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_flexifunction_command_t packet_in = {
-		5,
-	}72,
-	}139,
-	};
-	mavlink_flexifunction_command_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.command_type = packet_in.command_type;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_command_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_flexifunction_command_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_command_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.command_type );
-	mavlink_msg_flexifunction_command_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_command_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.command_type );
-	mavlink_msg_flexifunction_command_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_flexifunction_command_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_command_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.command_type );
-	mavlink_msg_flexifunction_command_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_flexifunction_command_ack(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_flexifunction_command_ack_t packet_in = {
-		17235,
-	}17339,
-	};
-	mavlink_flexifunction_command_ack_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.command_type = packet_in.command_type;
-        	packet1.result = packet_in.result;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_command_ack_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_flexifunction_command_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_command_ack_pack(system_id, component_id, &msg , packet1.command_type , packet1.result );
-	mavlink_msg_flexifunction_command_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_command_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command_type , packet1.result );
-	mavlink_msg_flexifunction_command_ack_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_flexifunction_command_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_flexifunction_command_ack_send(MAVLINK_COMM_1 , packet1.command_type , packet1.result );
-	mavlink_msg_flexifunction_command_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f2_a(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_udb_extra_f2_a_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}963498088,
-	}18067,
-	}18171,
-	}18275,
-	}18379,
-	}18483,
-	}18587,
-	}18691,
-	}18795,
-	}18899,
-	}19003,
-	}19107,
-	}19211,
-	}19315,
-	}19419,
-	}19523,
-	}19627,
-	}19731,
-	}19835,
-	}19939,
-	}20043,
-	}20147,
-	}20251,
-	}20355,
-	}63,
-	};
-	mavlink_serial_udb_extra_f2_a_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_time = packet_in.sue_time;
-        	packet1.sue_latitude = packet_in.sue_latitude;
-        	packet1.sue_longitude = packet_in.sue_longitude;
-        	packet1.sue_altitude = packet_in.sue_altitude;
-        	packet1.sue_waypoint_index = packet_in.sue_waypoint_index;
-        	packet1.sue_rmat0 = packet_in.sue_rmat0;
-        	packet1.sue_rmat1 = packet_in.sue_rmat1;
-        	packet1.sue_rmat2 = packet_in.sue_rmat2;
-        	packet1.sue_rmat3 = packet_in.sue_rmat3;
-        	packet1.sue_rmat4 = packet_in.sue_rmat4;
-        	packet1.sue_rmat5 = packet_in.sue_rmat5;
-        	packet1.sue_rmat6 = packet_in.sue_rmat6;
-        	packet1.sue_rmat7 = packet_in.sue_rmat7;
-        	packet1.sue_rmat8 = packet_in.sue_rmat8;
-        	packet1.sue_cog = packet_in.sue_cog;
-        	packet1.sue_sog = packet_in.sue_sog;
-        	packet1.sue_cpu_load = packet_in.sue_cpu_load;
-        	packet1.sue_voltage_milis = packet_in.sue_voltage_milis;
-        	packet1.sue_air_speed_3DIMU = packet_in.sue_air_speed_3DIMU;
-        	packet1.sue_estimated_wind_0 = packet_in.sue_estimated_wind_0;
-        	packet1.sue_estimated_wind_1 = packet_in.sue_estimated_wind_1;
-        	packet1.sue_estimated_wind_2 = packet_in.sue_estimated_wind_2;
-        	packet1.sue_magFieldEarth0 = packet_in.sue_magFieldEarth0;
-        	packet1.sue_magFieldEarth1 = packet_in.sue_magFieldEarth1;
-        	packet1.sue_magFieldEarth2 = packet_in.sue_magFieldEarth2;
-        	packet1.sue_svs = packet_in.sue_svs;
-        	packet1.sue_hdop = packet_in.sue_hdop;
-        	packet1.sue_status = packet_in.sue_status;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f2_a_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f2_a_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f2_a_pack(system_id, component_id, &msg , packet1.sue_time , packet1.sue_status , packet1.sue_latitude , packet1.sue_longitude , packet1.sue_altitude , packet1.sue_waypoint_index , packet1.sue_rmat0 , packet1.sue_rmat1 , packet1.sue_rmat2 , packet1.sue_rmat3 , packet1.sue_rmat4 , packet1.sue_rmat5 , packet1.sue_rmat6 , packet1.sue_rmat7 , packet1.sue_rmat8 , packet1.sue_cog , packet1.sue_sog , packet1.sue_cpu_load , packet1.sue_voltage_milis , packet1.sue_air_speed_3DIMU , packet1.sue_estimated_wind_0 , packet1.sue_estimated_wind_1 , packet1.sue_estimated_wind_2 , packet1.sue_magFieldEarth0 , packet1.sue_magFieldEarth1 , packet1.sue_magFieldEarth2 , packet1.sue_svs , packet1.sue_hdop );
-	mavlink_msg_serial_udb_extra_f2_a_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f2_a_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_time , packet1.sue_status , packet1.sue_latitude , packet1.sue_longitude , packet1.sue_altitude , packet1.sue_waypoint_index , packet1.sue_rmat0 , packet1.sue_rmat1 , packet1.sue_rmat2 , packet1.sue_rmat3 , packet1.sue_rmat4 , packet1.sue_rmat5 , packet1.sue_rmat6 , packet1.sue_rmat7 , packet1.sue_rmat8 , packet1.sue_cog , packet1.sue_sog , packet1.sue_cpu_load , packet1.sue_voltage_milis , packet1.sue_air_speed_3DIMU , packet1.sue_estimated_wind_0 , packet1.sue_estimated_wind_1 , packet1.sue_estimated_wind_2 , packet1.sue_magFieldEarth0 , packet1.sue_magFieldEarth1 , packet1.sue_magFieldEarth2 , packet1.sue_svs , packet1.sue_hdop );
-	mavlink_msg_serial_udb_extra_f2_a_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_udb_extra_f2_a_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f2_a_send(MAVLINK_COMM_1 , packet1.sue_time , packet1.sue_status , packet1.sue_latitude , packet1.sue_longitude , packet1.sue_altitude , packet1.sue_waypoint_index , packet1.sue_rmat0 , packet1.sue_rmat1 , packet1.sue_rmat2 , packet1.sue_rmat3 , packet1.sue_rmat4 , packet1.sue_rmat5 , packet1.sue_rmat6 , packet1.sue_rmat7 , packet1.sue_rmat8 , packet1.sue_cog , packet1.sue_sog , packet1.sue_cpu_load , packet1.sue_voltage_milis , packet1.sue_air_speed_3DIMU , packet1.sue_estimated_wind_0 , packet1.sue_estimated_wind_1 , packet1.sue_estimated_wind_2 , packet1.sue_magFieldEarth0 , packet1.sue_magFieldEarth1 , packet1.sue_magFieldEarth2 , packet1.sue_svs , packet1.sue_hdop );
-	mavlink_msg_serial_udb_extra_f2_a_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f2_b(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_udb_extra_f2_b_t packet_in = {
-		963497464,
-	}963497672,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	}18067,
-	}18171,
-	}18275,
-	}18379,
-	}18483,
-	}18587,
-	}18691,
-	}18795,
-	}18899,
-	}19003,
-	}19107,
-	}19211,
-	}19315,
-	}19419,
-	}19523,
-	}19627,
-	}19731,
-	}19835,
-	}19939,
-	}20043,
-	}20147,
-	}20251,
-	}20355,
-	}20459,
-	}20563,
-	}20667,
-	}20771,
-	};
-	mavlink_serial_udb_extra_f2_b_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_time = packet_in.sue_time;
-        	packet1.sue_flags = packet_in.sue_flags;
-        	packet1.sue_pwm_input_1 = packet_in.sue_pwm_input_1;
-        	packet1.sue_pwm_input_2 = packet_in.sue_pwm_input_2;
-        	packet1.sue_pwm_input_3 = packet_in.sue_pwm_input_3;
-        	packet1.sue_pwm_input_4 = packet_in.sue_pwm_input_4;
-        	packet1.sue_pwm_input_5 = packet_in.sue_pwm_input_5;
-        	packet1.sue_pwm_input_6 = packet_in.sue_pwm_input_6;
-        	packet1.sue_pwm_input_7 = packet_in.sue_pwm_input_7;
-        	packet1.sue_pwm_input_8 = packet_in.sue_pwm_input_8;
-        	packet1.sue_pwm_input_9 = packet_in.sue_pwm_input_9;
-        	packet1.sue_pwm_input_10 = packet_in.sue_pwm_input_10;
-        	packet1.sue_pwm_output_1 = packet_in.sue_pwm_output_1;
-        	packet1.sue_pwm_output_2 = packet_in.sue_pwm_output_2;
-        	packet1.sue_pwm_output_3 = packet_in.sue_pwm_output_3;
-        	packet1.sue_pwm_output_4 = packet_in.sue_pwm_output_4;
-        	packet1.sue_pwm_output_5 = packet_in.sue_pwm_output_5;
-        	packet1.sue_pwm_output_6 = packet_in.sue_pwm_output_6;
-        	packet1.sue_pwm_output_7 = packet_in.sue_pwm_output_7;
-        	packet1.sue_pwm_output_8 = packet_in.sue_pwm_output_8;
-        	packet1.sue_pwm_output_9 = packet_in.sue_pwm_output_9;
-        	packet1.sue_pwm_output_10 = packet_in.sue_pwm_output_10;
-        	packet1.sue_imu_location_x = packet_in.sue_imu_location_x;
-        	packet1.sue_imu_location_y = packet_in.sue_imu_location_y;
-        	packet1.sue_imu_location_z = packet_in.sue_imu_location_z;
-        	packet1.sue_osc_fails = packet_in.sue_osc_fails;
-        	packet1.sue_imu_velocity_x = packet_in.sue_imu_velocity_x;
-        	packet1.sue_imu_velocity_y = packet_in.sue_imu_velocity_y;
-        	packet1.sue_imu_velocity_z = packet_in.sue_imu_velocity_z;
-        	packet1.sue_waypoint_goal_x = packet_in.sue_waypoint_goal_x;
-        	packet1.sue_waypoint_goal_y = packet_in.sue_waypoint_goal_y;
-        	packet1.sue_waypoint_goal_z = packet_in.sue_waypoint_goal_z;
-        	packet1.sue_memory_stack_free = packet_in.sue_memory_stack_free;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f2_b_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f2_b_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f2_b_pack(system_id, component_id, &msg , packet1.sue_time , packet1.sue_pwm_input_1 , packet1.sue_pwm_input_2 , packet1.sue_pwm_input_3 , packet1.sue_pwm_input_4 , packet1.sue_pwm_input_5 , packet1.sue_pwm_input_6 , packet1.sue_pwm_input_7 , packet1.sue_pwm_input_8 , packet1.sue_pwm_input_9 , packet1.sue_pwm_input_10 , packet1.sue_pwm_output_1 , packet1.sue_pwm_output_2 , packet1.sue_pwm_output_3 , packet1.sue_pwm_output_4 , packet1.sue_pwm_output_5 , packet1.sue_pwm_output_6 , packet1.sue_pwm_output_7 , packet1.sue_pwm_output_8 , packet1.sue_pwm_output_9 , packet1.sue_pwm_output_10 , packet1.sue_imu_location_x , packet1.sue_imu_location_y , packet1.sue_imu_location_z , packet1.sue_flags , packet1.sue_osc_fails , packet1.sue_imu_velocity_x , packet1.sue_imu_velocity_y , packet1.sue_imu_velocity_z , packet1.sue_waypoint_goal_x , packet1.sue_waypoint_goal_y , packet1.sue_waypoint_goal_z , packet1.sue_memory_stack_free );
-	mavlink_msg_serial_udb_extra_f2_b_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f2_b_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_time , packet1.sue_pwm_input_1 , packet1.sue_pwm_input_2 , packet1.sue_pwm_input_3 , packet1.sue_pwm_input_4 , packet1.sue_pwm_input_5 , packet1.sue_pwm_input_6 , packet1.sue_pwm_input_7 , packet1.sue_pwm_input_8 , packet1.sue_pwm_input_9 , packet1.sue_pwm_input_10 , packet1.sue_pwm_output_1 , packet1.sue_pwm_output_2 , packet1.sue_pwm_output_3 , packet1.sue_pwm_output_4 , packet1.sue_pwm_output_5 , packet1.sue_pwm_output_6 , packet1.sue_pwm_output_7 , packet1.sue_pwm_output_8 , packet1.sue_pwm_output_9 , packet1.sue_pwm_output_10 , packet1.sue_imu_location_x , packet1.sue_imu_location_y , packet1.sue_imu_location_z , packet1.sue_flags , packet1.sue_osc_fails , packet1.sue_imu_velocity_x , packet1.sue_imu_velocity_y , packet1.sue_imu_velocity_z , packet1.sue_waypoint_goal_x , packet1.sue_waypoint_goal_y , packet1.sue_waypoint_goal_z , packet1.sue_memory_stack_free );
-	mavlink_msg_serial_udb_extra_f2_b_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_udb_extra_f2_b_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f2_b_send(MAVLINK_COMM_1 , packet1.sue_time , packet1.sue_pwm_input_1 , packet1.sue_pwm_input_2 , packet1.sue_pwm_input_3 , packet1.sue_pwm_input_4 , packet1.sue_pwm_input_5 , packet1.sue_pwm_input_6 , packet1.sue_pwm_input_7 , packet1.sue_pwm_input_8 , packet1.sue_pwm_input_9 , packet1.sue_pwm_input_10 , packet1.sue_pwm_output_1 , packet1.sue_pwm_output_2 , packet1.sue_pwm_output_3 , packet1.sue_pwm_output_4 , packet1.sue_pwm_output_5 , packet1.sue_pwm_output_6 , packet1.sue_pwm_output_7 , packet1.sue_pwm_output_8 , packet1.sue_pwm_output_9 , packet1.sue_pwm_output_10 , packet1.sue_imu_location_x , packet1.sue_imu_location_y , packet1.sue_imu_location_z , packet1.sue_flags , packet1.sue_osc_fails , packet1.sue_imu_velocity_x , packet1.sue_imu_velocity_y , packet1.sue_imu_velocity_z , packet1.sue_waypoint_goal_x , packet1.sue_waypoint_goal_y , packet1.sue_waypoint_goal_z , packet1.sue_memory_stack_free );
-	mavlink_msg_serial_udb_extra_f2_b_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f4(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_udb_extra_f4_t packet_in = {
-		5,
-	}72,
-	}139,
-	}206,
-	}17,
-	}84,
-	}151,
-	}218,
-	}29,
-	}96,
-	};
-	mavlink_serial_udb_extra_f4_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_ROLL_STABILIZATION_AILERONS = packet_in.sue_ROLL_STABILIZATION_AILERONS;
-        	packet1.sue_ROLL_STABILIZATION_RUDDER = packet_in.sue_ROLL_STABILIZATION_RUDDER;
-        	packet1.sue_PITCH_STABILIZATION = packet_in.sue_PITCH_STABILIZATION;
-        	packet1.sue_YAW_STABILIZATION_RUDDER = packet_in.sue_YAW_STABILIZATION_RUDDER;
-        	packet1.sue_YAW_STABILIZATION_AILERON = packet_in.sue_YAW_STABILIZATION_AILERON;
-        	packet1.sue_AILERON_NAVIGATION = packet_in.sue_AILERON_NAVIGATION;
-        	packet1.sue_RUDDER_NAVIGATION = packet_in.sue_RUDDER_NAVIGATION;
-        	packet1.sue_ALTITUDEHOLD_STABILIZED = packet_in.sue_ALTITUDEHOLD_STABILIZED;
-        	packet1.sue_ALTITUDEHOLD_WAYPOINT = packet_in.sue_ALTITUDEHOLD_WAYPOINT;
-        	packet1.sue_RACING_MODE = packet_in.sue_RACING_MODE;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f4_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f4_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f4_pack(system_id, component_id, &msg , packet1.sue_ROLL_STABILIZATION_AILERONS , packet1.sue_ROLL_STABILIZATION_RUDDER , packet1.sue_PITCH_STABILIZATION , packet1.sue_YAW_STABILIZATION_RUDDER , packet1.sue_YAW_STABILIZATION_AILERON , packet1.sue_AILERON_NAVIGATION , packet1.sue_RUDDER_NAVIGATION , packet1.sue_ALTITUDEHOLD_STABILIZED , packet1.sue_ALTITUDEHOLD_WAYPOINT , packet1.sue_RACING_MODE );
-	mavlink_msg_serial_udb_extra_f4_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f4_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_ROLL_STABILIZATION_AILERONS , packet1.sue_ROLL_STABILIZATION_RUDDER , packet1.sue_PITCH_STABILIZATION , packet1.sue_YAW_STABILIZATION_RUDDER , packet1.sue_YAW_STABILIZATION_AILERON , packet1.sue_AILERON_NAVIGATION , packet1.sue_RUDDER_NAVIGATION , packet1.sue_ALTITUDEHOLD_STABILIZED , packet1.sue_ALTITUDEHOLD_WAYPOINT , packet1.sue_RACING_MODE );
-	mavlink_msg_serial_udb_extra_f4_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_udb_extra_f4_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f4_send(MAVLINK_COMM_1 , packet1.sue_ROLL_STABILIZATION_AILERONS , packet1.sue_ROLL_STABILIZATION_RUDDER , packet1.sue_PITCH_STABILIZATION , packet1.sue_YAW_STABILIZATION_RUDDER , packet1.sue_YAW_STABILIZATION_AILERON , packet1.sue_AILERON_NAVIGATION , packet1.sue_RUDDER_NAVIGATION , packet1.sue_ALTITUDEHOLD_STABILIZED , packet1.sue_ALTITUDEHOLD_WAYPOINT , packet1.sue_RACING_MODE );
-	mavlink_msg_serial_udb_extra_f4_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f5(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_udb_extra_f5_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	};
-	mavlink_serial_udb_extra_f5_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_YAWKP_AILERON = packet_in.sue_YAWKP_AILERON;
-        	packet1.sue_YAWKD_AILERON = packet_in.sue_YAWKD_AILERON;
-        	packet1.sue_ROLLKP = packet_in.sue_ROLLKP;
-        	packet1.sue_ROLLKD = packet_in.sue_ROLLKD;
-        	packet1.sue_YAW_STABILIZATION_AILERON = packet_in.sue_YAW_STABILIZATION_AILERON;
-        	packet1.sue_AILERON_BOOST = packet_in.sue_AILERON_BOOST;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f5_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f5_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f5_pack(system_id, component_id, &msg , packet1.sue_YAWKP_AILERON , packet1.sue_YAWKD_AILERON , packet1.sue_ROLLKP , packet1.sue_ROLLKD , packet1.sue_YAW_STABILIZATION_AILERON , packet1.sue_AILERON_BOOST );
-	mavlink_msg_serial_udb_extra_f5_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f5_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_YAWKP_AILERON , packet1.sue_YAWKD_AILERON , packet1.sue_ROLLKP , packet1.sue_ROLLKD , packet1.sue_YAW_STABILIZATION_AILERON , packet1.sue_AILERON_BOOST );
-	mavlink_msg_serial_udb_extra_f5_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_udb_extra_f5_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f5_send(MAVLINK_COMM_1 , packet1.sue_YAWKP_AILERON , packet1.sue_YAWKD_AILERON , packet1.sue_ROLLKP , packet1.sue_ROLLKD , packet1.sue_YAW_STABILIZATION_AILERON , packet1.sue_AILERON_BOOST );
-	mavlink_msg_serial_udb_extra_f5_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f6(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_udb_extra_f6_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	};
-	mavlink_serial_udb_extra_f6_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_PITCHGAIN = packet_in.sue_PITCHGAIN;
-        	packet1.sue_PITCHKD = packet_in.sue_PITCHKD;
-        	packet1.sue_RUDDER_ELEV_MIX = packet_in.sue_RUDDER_ELEV_MIX;
-        	packet1.sue_ROLL_ELEV_MIX = packet_in.sue_ROLL_ELEV_MIX;
-        	packet1.sue_ELEVATOR_BOOST = packet_in.sue_ELEVATOR_BOOST;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f6_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f6_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f6_pack(system_id, component_id, &msg , packet1.sue_PITCHGAIN , packet1.sue_PITCHKD , packet1.sue_RUDDER_ELEV_MIX , packet1.sue_ROLL_ELEV_MIX , packet1.sue_ELEVATOR_BOOST );
-	mavlink_msg_serial_udb_extra_f6_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f6_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_PITCHGAIN , packet1.sue_PITCHKD , packet1.sue_RUDDER_ELEV_MIX , packet1.sue_ROLL_ELEV_MIX , packet1.sue_ELEVATOR_BOOST );
-	mavlink_msg_serial_udb_extra_f6_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_udb_extra_f6_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f6_send(MAVLINK_COMM_1 , packet1.sue_PITCHGAIN , packet1.sue_PITCHKD , packet1.sue_RUDDER_ELEV_MIX , packet1.sue_ROLL_ELEV_MIX , packet1.sue_ELEVATOR_BOOST );
-	mavlink_msg_serial_udb_extra_f6_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f7(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_udb_extra_f7_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	};
-	mavlink_serial_udb_extra_f7_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_YAWKP_RUDDER = packet_in.sue_YAWKP_RUDDER;
-        	packet1.sue_YAWKD_RUDDER = packet_in.sue_YAWKD_RUDDER;
-        	packet1.sue_ROLLKP_RUDDER = packet_in.sue_ROLLKP_RUDDER;
-        	packet1.sue_ROLLKD_RUDDER = packet_in.sue_ROLLKD_RUDDER;
-        	packet1.sue_RUDDER_BOOST = packet_in.sue_RUDDER_BOOST;
-        	packet1.sue_RTL_PITCH_DOWN = packet_in.sue_RTL_PITCH_DOWN;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f7_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f7_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f7_pack(system_id, component_id, &msg , packet1.sue_YAWKP_RUDDER , packet1.sue_YAWKD_RUDDER , packet1.sue_ROLLKP_RUDDER , packet1.sue_ROLLKD_RUDDER , packet1.sue_RUDDER_BOOST , packet1.sue_RTL_PITCH_DOWN );
-	mavlink_msg_serial_udb_extra_f7_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f7_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_YAWKP_RUDDER , packet1.sue_YAWKD_RUDDER , packet1.sue_ROLLKP_RUDDER , packet1.sue_ROLLKD_RUDDER , packet1.sue_RUDDER_BOOST , packet1.sue_RTL_PITCH_DOWN );
-	mavlink_msg_serial_udb_extra_f7_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_udb_extra_f7_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f7_send(MAVLINK_COMM_1 , packet1.sue_YAWKP_RUDDER , packet1.sue_YAWKD_RUDDER , packet1.sue_ROLLKP_RUDDER , packet1.sue_ROLLKD_RUDDER , packet1.sue_RUDDER_BOOST , packet1.sue_RTL_PITCH_DOWN );
-	mavlink_msg_serial_udb_extra_f7_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f8(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_udb_extra_f8_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	};
-	mavlink_serial_udb_extra_f8_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_HEIGHT_TARGET_MAX = packet_in.sue_HEIGHT_TARGET_MAX;
-        	packet1.sue_HEIGHT_TARGET_MIN = packet_in.sue_HEIGHT_TARGET_MIN;
-        	packet1.sue_ALT_HOLD_THROTTLE_MIN = packet_in.sue_ALT_HOLD_THROTTLE_MIN;
-        	packet1.sue_ALT_HOLD_THROTTLE_MAX = packet_in.sue_ALT_HOLD_THROTTLE_MAX;
-        	packet1.sue_ALT_HOLD_PITCH_MIN = packet_in.sue_ALT_HOLD_PITCH_MIN;
-        	packet1.sue_ALT_HOLD_PITCH_MAX = packet_in.sue_ALT_HOLD_PITCH_MAX;
-        	packet1.sue_ALT_HOLD_PITCH_HIGH = packet_in.sue_ALT_HOLD_PITCH_HIGH;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f8_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f8_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f8_pack(system_id, component_id, &msg , packet1.sue_HEIGHT_TARGET_MAX , packet1.sue_HEIGHT_TARGET_MIN , packet1.sue_ALT_HOLD_THROTTLE_MIN , packet1.sue_ALT_HOLD_THROTTLE_MAX , packet1.sue_ALT_HOLD_PITCH_MIN , packet1.sue_ALT_HOLD_PITCH_MAX , packet1.sue_ALT_HOLD_PITCH_HIGH );
-	mavlink_msg_serial_udb_extra_f8_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f8_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_HEIGHT_TARGET_MAX , packet1.sue_HEIGHT_TARGET_MIN , packet1.sue_ALT_HOLD_THROTTLE_MIN , packet1.sue_ALT_HOLD_THROTTLE_MAX , packet1.sue_ALT_HOLD_PITCH_MIN , packet1.sue_ALT_HOLD_PITCH_MAX , packet1.sue_ALT_HOLD_PITCH_HIGH );
-	mavlink_msg_serial_udb_extra_f8_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_udb_extra_f8_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f8_send(MAVLINK_COMM_1 , packet1.sue_HEIGHT_TARGET_MAX , packet1.sue_HEIGHT_TARGET_MIN , packet1.sue_ALT_HOLD_THROTTLE_MIN , packet1.sue_ALT_HOLD_THROTTLE_MAX , packet1.sue_ALT_HOLD_PITCH_MIN , packet1.sue_ALT_HOLD_PITCH_MAX , packet1.sue_ALT_HOLD_PITCH_HIGH );
-	mavlink_msg_serial_udb_extra_f8_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f13(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_udb_extra_f13_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}17859,
-	};
-	mavlink_serial_udb_extra_f13_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_lat_origin = packet_in.sue_lat_origin;
-        	packet1.sue_lon_origin = packet_in.sue_lon_origin;
-        	packet1.sue_alt_origin = packet_in.sue_alt_origin;
-        	packet1.sue_week_no = packet_in.sue_week_no;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f13_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f13_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f13_pack(system_id, component_id, &msg , packet1.sue_week_no , packet1.sue_lat_origin , packet1.sue_lon_origin , packet1.sue_alt_origin );
-	mavlink_msg_serial_udb_extra_f13_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f13_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_week_no , packet1.sue_lat_origin , packet1.sue_lon_origin , packet1.sue_alt_origin );
-	mavlink_msg_serial_udb_extra_f13_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_udb_extra_f13_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f13_send(MAVLINK_COMM_1 , packet1.sue_week_no , packet1.sue_lat_origin , packet1.sue_lon_origin , packet1.sue_alt_origin );
-	mavlink_msg_serial_udb_extra_f13_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f14(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_udb_extra_f14_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}163,
-	}230,
-	}41,
-	}108,
-	}175,
-	}242,
-	}53,
-	};
-	mavlink_serial_udb_extra_f14_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.sue_TRAP_SOURCE = packet_in.sue_TRAP_SOURCE;
-        	packet1.sue_RCON = packet_in.sue_RCON;
-        	packet1.sue_TRAP_FLAGS = packet_in.sue_TRAP_FLAGS;
-        	packet1.sue_osc_fail_count = packet_in.sue_osc_fail_count;
-        	packet1.sue_WIND_ESTIMATION = packet_in.sue_WIND_ESTIMATION;
-        	packet1.sue_GPS_TYPE = packet_in.sue_GPS_TYPE;
-        	packet1.sue_DR = packet_in.sue_DR;
-        	packet1.sue_BOARD_TYPE = packet_in.sue_BOARD_TYPE;
-        	packet1.sue_AIRFRAME = packet_in.sue_AIRFRAME;
-        	packet1.sue_CLOCK_CONFIG = packet_in.sue_CLOCK_CONFIG;
-        	packet1.sue_FLIGHT_PLAN_TYPE = packet_in.sue_FLIGHT_PLAN_TYPE;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f14_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f14_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f14_pack(system_id, component_id, &msg , packet1.sue_WIND_ESTIMATION , packet1.sue_GPS_TYPE , packet1.sue_DR , packet1.sue_BOARD_TYPE , packet1.sue_AIRFRAME , packet1.sue_RCON , packet1.sue_TRAP_FLAGS , packet1.sue_TRAP_SOURCE , packet1.sue_osc_fail_count , packet1.sue_CLOCK_CONFIG , packet1.sue_FLIGHT_PLAN_TYPE );
-	mavlink_msg_serial_udb_extra_f14_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f14_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_WIND_ESTIMATION , packet1.sue_GPS_TYPE , packet1.sue_DR , packet1.sue_BOARD_TYPE , packet1.sue_AIRFRAME , packet1.sue_RCON , packet1.sue_TRAP_FLAGS , packet1.sue_TRAP_SOURCE , packet1.sue_osc_fail_count , packet1.sue_CLOCK_CONFIG , packet1.sue_FLIGHT_PLAN_TYPE );
-	mavlink_msg_serial_udb_extra_f14_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_udb_extra_f14_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f14_send(MAVLINK_COMM_1 , packet1.sue_WIND_ESTIMATION , packet1.sue_GPS_TYPE , packet1.sue_DR , packet1.sue_BOARD_TYPE , packet1.sue_AIRFRAME , packet1.sue_RCON , packet1.sue_TRAP_FLAGS , packet1.sue_TRAP_SOURCE , packet1.sue_osc_fail_count , packet1.sue_CLOCK_CONFIG , packet1.sue_FLIGHT_PLAN_TYPE );
-	mavlink_msg_serial_udb_extra_f14_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f15(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_udb_extra_f15_t packet_in = {
-		{ 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44 },
-	}{ 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144 },
-	};
-	mavlink_serial_udb_extra_f15_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.sue_ID_VEHICLE_MODEL_NAME, packet_in.sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40);
-        	mav_array_memcpy(packet1.sue_ID_VEHICLE_REGISTRATION, packet_in.sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f15_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f15_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f15_pack(system_id, component_id, &msg , packet1.sue_ID_VEHICLE_MODEL_NAME , packet1.sue_ID_VEHICLE_REGISTRATION );
-	mavlink_msg_serial_udb_extra_f15_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f15_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_ID_VEHICLE_MODEL_NAME , packet1.sue_ID_VEHICLE_REGISTRATION );
-	mavlink_msg_serial_udb_extra_f15_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_udb_extra_f15_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f15_send(MAVLINK_COMM_1 , packet1.sue_ID_VEHICLE_MODEL_NAME , packet1.sue_ID_VEHICLE_REGISTRATION );
-	mavlink_msg_serial_udb_extra_f15_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_serial_udb_extra_f16(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_udb_extra_f16_t packet_in = {
-		{ 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44 },
-	}{ 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, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194 },
-	};
-	mavlink_serial_udb_extra_f16_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.sue_ID_LEAD_PILOT, packet_in.sue_ID_LEAD_PILOT, sizeof(uint8_t)*40);
-        	mav_array_memcpy(packet1.sue_ID_DIY_DRONES_URL, packet_in.sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f16_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_serial_udb_extra_f16_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f16_pack(system_id, component_id, &msg , packet1.sue_ID_LEAD_PILOT , packet1.sue_ID_DIY_DRONES_URL );
-	mavlink_msg_serial_udb_extra_f16_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f16_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sue_ID_LEAD_PILOT , packet1.sue_ID_DIY_DRONES_URL );
-	mavlink_msg_serial_udb_extra_f16_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_udb_extra_f16_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_serial_udb_extra_f16_send(MAVLINK_COMM_1 , packet1.sue_ID_LEAD_PILOT , packet1.sue_ID_DIY_DRONES_URL );
-	mavlink_msg_serial_udb_extra_f16_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_altitudes(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_altitudes_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	}963498088,
-	}963498296,
-	}963498504,
-	}963498712,
-	};
-	mavlink_altitudes_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.alt_gps = packet_in.alt_gps;
-        	packet1.alt_imu = packet_in.alt_imu;
-        	packet1.alt_barometric = packet_in.alt_barometric;
-        	packet1.alt_optical_flow = packet_in.alt_optical_flow;
-        	packet1.alt_range_finder = packet_in.alt_range_finder;
-        	packet1.alt_extra = packet_in.alt_extra;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_altitudes_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_altitudes_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_altitudes_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.alt_gps , packet1.alt_imu , packet1.alt_barometric , packet1.alt_optical_flow , packet1.alt_range_finder , packet1.alt_extra );
-	mavlink_msg_altitudes_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_altitudes_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.alt_gps , packet1.alt_imu , packet1.alt_barometric , packet1.alt_optical_flow , packet1.alt_range_finder , packet1.alt_extra );
-	mavlink_msg_altitudes_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_altitudes_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_altitudes_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.alt_gps , packet1.alt_imu , packet1.alt_barometric , packet1.alt_optical_flow , packet1.alt_range_finder , packet1.alt_extra );
-	mavlink_msg_altitudes_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_airspeeds(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_airspeeds_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	};
-	mavlink_airspeeds_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.time_boot_ms = packet_in.time_boot_ms;
-        	packet1.airspeed_imu = packet_in.airspeed_imu;
-        	packet1.airspeed_pitot = packet_in.airspeed_pitot;
-        	packet1.airspeed_hot_wire = packet_in.airspeed_hot_wire;
-        	packet1.airspeed_ultrasonic = packet_in.airspeed_ultrasonic;
-        	packet1.aoa = packet_in.aoa;
-        	packet1.aoy = packet_in.aoy;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_airspeeds_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_airspeeds_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_airspeeds_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.airspeed_imu , packet1.airspeed_pitot , packet1.airspeed_hot_wire , packet1.airspeed_ultrasonic , packet1.aoa , packet1.aoy );
-	mavlink_msg_airspeeds_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_airspeeds_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.airspeed_imu , packet1.airspeed_pitot , packet1.airspeed_hot_wire , packet1.airspeed_ultrasonic , packet1.aoa , packet1.aoy );
-	mavlink_msg_airspeeds_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_airspeeds_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_airspeeds_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.airspeed_imu , packet1.airspeed_pitot , packet1.airspeed_hot_wire , packet1.airspeed_ultrasonic , packet1.aoa , packet1.aoy );
-	mavlink_msg_airspeeds_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_matrixpilot(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_flexifunction_set(system_id, component_id, last_msg);
-	mavlink_test_flexifunction_read_req(system_id, component_id, last_msg);
-	mavlink_test_flexifunction_buffer_function(system_id, component_id, last_msg);
-	mavlink_test_flexifunction_buffer_function_ack(system_id, component_id, last_msg);
-	mavlink_test_flexifunction_directory(system_id, component_id, last_msg);
-	mavlink_test_flexifunction_directory_ack(system_id, component_id, last_msg);
-	mavlink_test_flexifunction_command(system_id, component_id, last_msg);
-	mavlink_test_flexifunction_command_ack(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f2_a(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f2_b(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f4(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f5(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f6(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f7(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f8(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f13(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f14(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f15(system_id, component_id, last_msg);
-	mavlink_test_serial_udb_extra_f16(system_id, component_id, last_msg);
-	mavlink_test_altitudes(system_id, component_id, last_msg);
-	mavlink_test_airspeeds(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // MATRIXPILOT_TESTSUITE_H
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h
deleted file mode 100644
index a8be16c42..000000000
--- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from matrixpilot.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:04 2014"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
- 
-#endif // MAVLINK_VERSION_H
diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h
deleted file mode 100644
index 51afac87c..000000000
--- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h
+++ /dev/null
@@ -1,135 +0,0 @@
-#ifndef  _MAVLINK_CONVERSIONS_H_
-#define  _MAVLINK_CONVERSIONS_H_
-
-/* enable math defines on Windows */
-#ifdef _MSC_VER
-#ifndef _USE_MATH_DEFINES
-#define _USE_MATH_DEFINES
-#endif
-#endif
-#include <math.h>
-
-#ifndef M_PI_2
-    #define M_PI_2 ((float)asin(1))
-#endif
-
-/**
- * @file mavlink_conversions.h
- *
- * These conversion functions follow the NASA rotation standards definition file
- * available online.
- *
- * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
- * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
- * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
- * protocol as widely as possible.
- *
- * @author James Goppert
- */
-
-MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])
-{
-    double a = quaternion[0];
-    double b = quaternion[1];
-    double c = quaternion[2];
-    double d = quaternion[3];
-    double aSq = a * a;
-    double bSq = b * b;
-    double cSq = c * c;
-    double dSq = d * d;
-    dcm[0][0] = aSq + bSq - cSq - dSq;
-    dcm[0][1] = 2.0 * (b * c - a * d);
-    dcm[0][2] = 2.0 * (a * c + b * d);
-    dcm[1][0] = 2.0 * (b * c + a * d);
-    dcm[1][1] = aSq - bSq + cSq - dSq;
-    dcm[1][2] = 2.0 * (c * d - a * b);
-    dcm[2][0] = 2.0 * (b * d - a * c);
-    dcm[2][1] = 2.0 * (a * b + c * d);
-    dcm[2][2] = aSq - bSq - cSq + dSq;
-}
-
-MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)
-{
-    float phi, theta, psi;
-    theta = asin(-dcm[2][0]);
-
-    if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
-        phi = 0.0f;
-        psi = (atan2f(dcm[1][2] - dcm[0][1],
-                dcm[0][2] + dcm[1][1]) + phi);
-
-    } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
-        phi = 0.0f;
-        psi = atan2f(dcm[1][2] - dcm[0][1],
-                  dcm[0][2] + dcm[1][1] - phi);
-
-    } else {
-        phi = atan2f(dcm[2][1], dcm[2][2]);
-        psi = atan2f(dcm[1][0], dcm[0][0]);
-    }
-
-    *roll = phi;
-    *pitch = theta;
-    *yaw = psi;
-}
-
-MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)
-{
-    float dcm[3][3];
-    mavlink_quaternion_to_dcm(quaternion, dcm);
-    mavlink_dcm_to_euler(dcm, roll, pitch, yaw);
-}
-
-MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
-{
-    double cosPhi_2 = cos((double)roll / 2.0);
-    double sinPhi_2 = sin((double)roll / 2.0);
-    double cosTheta_2 = cos((double)pitch / 2.0);
-    double sinTheta_2 = sin((double)pitch / 2.0);
-    double cosPsi_2 = cos((double)yaw / 2.0);
-    double sinPsi_2 = sin((double)yaw / 2.0);
-    quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
-            sinPhi_2 * sinTheta_2 * sinPsi_2);
-    quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
-            cosPhi_2 * sinTheta_2 * sinPsi_2);
-    quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
-            sinPhi_2 * cosTheta_2 * sinPsi_2);
-    quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
-            sinPhi_2 * sinTheta_2 * cosPsi_2);
-}
-
-MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
-{
-    quaternion[0] = (0.5 * sqrt(1.0 +
-            (double)(dcm[0][0] + dcm[1][1] + dcm[2][2])));
-    quaternion[1] = (0.5 * sqrt(1.0 +
-            (double)(dcm[0][0] - dcm[1][1] - dcm[2][2])));
-    quaternion[2] = (0.5 * sqrt(1.0 +
-            (double)(-dcm[0][0] + dcm[1][1] - dcm[2][2])));
-    quaternion[3] = (0.5 * sqrt(1.0 +
-            (double)(-dcm[0][0] - dcm[1][1] + dcm[2][2])));
-}
-
-MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
-{
-    double cosPhi = cos(roll);
-    double sinPhi = sin(roll);
-    double cosThe = cos(pitch);
-    double sinThe = sin(pitch);
-    double cosPsi = cos(yaw);
-    double sinPsi = sin(yaw);
-
-    dcm[0][0] = cosThe * cosPsi;
-    dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
-    dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
-
-    dcm[1][0] = cosThe * sinPsi;
-    dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
-    dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
-
-    dcm[2][0] = -sinThe;
-    dcm[2][1] = sinPhi * cosThe;
-    dcm[2][2] = cosPhi * cosThe;
-}
-
-#endif
diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h
deleted file mode 100644
index 96672f847..000000000
--- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h
+++ /dev/null
@@ -1,567 +0,0 @@
-#ifndef  _MAVLINK_HELPERS_H_
-#define  _MAVLINK_HELPERS_H_
-
-#include "string.h"
-#include "checksum.h"
-#include "mavlink_types.h"
-#include "mavlink_conversions.h"
-
-#ifndef MAVLINK_HELPER
-#define MAVLINK_HELPER
-#endif
-
-/*
- * Internal function to give access to the channel status for each channel
- */
-#ifndef MAVLINK_GET_CHANNEL_STATUS
-MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
-{
-#if MAVLINK_EXTERNAL_RX_STATUS
-	// No m_mavlink_status array defined in function,
-	// has to be defined externally
-#else
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-#endif
-	return &m_mavlink_status[chan];
-}
-#endif
-
-/*
- * Internal function to give access to the channel buffer for each channel
- */
-#ifndef MAVLINK_GET_CHANNEL_BUFFER
-MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
-{
-	
-#if MAVLINK_EXTERNAL_RX_BUFFER
-	// No m_mavlink_buffer array defined in function,
-	// has to be defined externally
-#else
-	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
-#endif
-	return &m_mavlink_buffer[chan];
-}
-#endif
-
-/**
- * @brief Reset the status of a channel.
- */
-MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
-{
-	mavlink_status_t *status = mavlink_get_channel_status(chan);
-	status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-}
-
-/**
- * @brief Finalize a MAVLink message with channel assignment
- *
- * This function calculates the checksum and sets length and aircraft id correctly.
- * It assumes that the message id and the payload are already correctly set. This function
- * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
- * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
- *
- * @param msg Message to finalize
- * @param system_id Id of the sending (this) system, 1-127
- * @param length Message length
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length, uint8_t crc_extra)
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length)
-#endif
-{
-	// This code part is the same for all messages;
-	uint16_t checksum;
-	msg->magic = MAVLINK_STX;
-	msg->len = length;
-	msg->sysid = system_id;
-	msg->compid = component_id;
-	// One sequence number per component
-	msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
-	mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
-	checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
-#if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
-#endif
-	mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
-	mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
-
-	return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-}
-
-
-/**
- * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length, uint8_t crc_extra)
-{
-	return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
-}
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length)
-{
-	return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
-}
-#endif
-
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
-
-/**
- * @brief Finalize a MAVLink message with channel assignment and send
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
-						    uint8_t length, uint8_t crc_extra)
-#else
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
-#endif
-{
-	uint16_t checksum;
-	uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
-	uint8_t ck[2];
-	mavlink_status_t *status = mavlink_get_channel_status(chan);
-	buf[0] = MAVLINK_STX;
-	buf[1] = length;
-	buf[2] = status->current_tx_seq;
-	buf[3] = mavlink_system.sysid;
-	buf[4] = mavlink_system.compid;
-	buf[5] = msgid;
-	status->current_tx_seq++;
-	checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
-	crc_accumulate_buffer(&checksum, packet, length);
-#if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
-#endif
-	ck[0] = (uint8_t)(checksum & 0xFF);
-	ck[1] = (uint8_t)(checksum >> 8);
-
-	MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
-	_mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
-	_mavlink_send_uart(chan, packet, length);
-	_mavlink_send_uart(chan, (const char *)ck, 2);
-	MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
-}
-
-/**
- * @brief re-send a message over a uart channel
- * this is more stack efficient than re-marshalling the message
- */
-MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
-{
-	uint8_t ck[2];
-
-	ck[0] = (uint8_t)(msg->checksum & 0xFF);
-	ck[1] = (uint8_t)(msg->checksum >> 8);
-
-	MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
-	_mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
-	_mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
-	_mavlink_send_uart(chan, (const char *)ck, 2);
-	MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
-}
-#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-/**
- * @brief Pack a message to send it over a serial byte stream
- */
-MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
-{
-	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
-	return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
-}
-
-union __mavlink_bitfield {
-	uint8_t uint8;
-	int8_t int8;
-	uint16_t uint16;
-	int16_t int16;
-	uint32_t uint32;
-	int32_t int32;
-};
-
-
-MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
-{
-	crc_init(&msg->checksum);
-}
-
-MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
-{
-	crc_accumulate(c, &msg->checksum);
-}
-
-/**
- * This is a convenience function which handles the complete MAVLink parsing.
- * the function will parse one byte at a time and return the complete packet once
- * it could be successfully decoded. Checksum and other failures will be silently
- * ignored.
- *
- * @param chan     ID of the current channel. This allows to parse different channels with this function.
- *                 a channel is not a physical message channel like a serial port, but a logic partition of
- *                 the communication streams in this case. COMM_NB is the limit for the number of channels
- *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
- * @param c        The char to barse
- *
- * @param returnMsg NULL if no message could be decoded, the message data else
- * @return 0 if no message could be decoded, 1 else
- *
- * A typical use scenario of this function call is:
- *
- * @code
- * #include <inttypes.h> // For fixed-width uint8_t type
- *
- * mavlink_message_t msg;
- * int chan = 0;
- *
- *
- * while(serial.bytesAvailable > 0)
- * {
- *   uint8_t byte = serial.getNextByte();
- *   if (mavlink_parse_char(chan, byte, &msg))
- *     {
- *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
- *     }
- * }
- *
- *
- * @endcode
- */
-MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
-{
-        /*
-	  default message crc function. You can override this per-system to
-	  put this data in a different memory segment
-	*/
-#if MAVLINK_CRC_EXTRA
-#ifndef MAVLINK_MESSAGE_CRC
-	static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
-#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
-#endif
-#endif
-
-/* Enable this option to check the length of each message.
- This allows invalid messages to be caught much sooner. Use if the transmission
- medium is prone to missing (or extra) characters (e.g. a radio that fades in
- and out). Only use if the channel will only contain messages types listed in
- the headers.
-*/
-#if MAVLINK_CHECK_MESSAGE_LENGTH
-#ifndef MAVLINK_MESSAGE_LENGTH
-	static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
-#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
-#endif
-#endif
-
-	mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message
-	mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
-	int bufferIndex = 0;
-
-	status->msg_received = 0;
-
-	switch (status->parse_state)
-	{
-	case MAVLINK_PARSE_STATE_UNINIT:
-	case MAVLINK_PARSE_STATE_IDLE:
-		if (c == MAVLINK_STX)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-			rxmsg->len = 0;
-			rxmsg->magic = c;
-			mavlink_start_checksum(rxmsg);
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_STX:
-			if (status->msg_received 
-/* Support shorter buffers than the
-   default maximum packet size */
-#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
-				|| c > MAVLINK_MAX_PAYLOAD_LEN
-#endif
-				)
-		{
-			status->buffer_overrun++;
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-		}
-		else
-		{
-			// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
-			rxmsg->len = c;
-			status->packet_idx = 0;
-			mavlink_update_checksum(rxmsg, c);
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_LENGTH:
-		rxmsg->seq = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_SEQ:
-		rxmsg->sysid = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_SYSID:
-		rxmsg->compid = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_COMPID:
-#if MAVLINK_CHECK_MESSAGE_LENGTH
-	        if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
-		{
-			status->parse_error++;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			break;
-			if (c == MAVLINK_STX)
-			{
-				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-				mavlink_start_checksum(rxmsg);
-			}
-	        }
-#endif
-		rxmsg->msgid = c;
-		mavlink_update_checksum(rxmsg, c);
-		if (rxmsg->len == 0)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
-		}
-		else
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_MSGID:
-		_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
-		mavlink_update_checksum(rxmsg, c);
-		if (status->packet_idx == rxmsg->len)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
-#if MAVLINK_CRC_EXTRA
-		mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
-#endif
-		if (c != (rxmsg->checksum & 0xFF)) {
-			// Check first checksum byte
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			if (c == MAVLINK_STX)
-			{
-				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-				rxmsg->len = 0;
-				mavlink_start_checksum(rxmsg);
-			}
-		}
-		else
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
-			_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_CRC1:
-		if (c != (rxmsg->checksum >> 8)) {
-			// Check second checksum byte
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			if (c == MAVLINK_STX)
-			{
-				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-				rxmsg->len = 0;
-				mavlink_start_checksum(rxmsg);
-			}
-		}
-		else
-		{
-			// Successfully got message
-			status->msg_received = 1;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
-			memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
-		}
-		break;
-	}
-
-	bufferIndex++;
-	// If a message has been sucessfully decoded, check index
-	if (status->msg_received == 1)
-	{
-		//while(status->current_seq != rxmsg->seq)
-		//{
-		//	status->packet_rx_drop_count++;
-		//               status->current_seq++;
-		//}
-		status->current_rx_seq = rxmsg->seq;
-		// Initial condition: If no packet has been received so far, drop count is undefined
-		if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
-		// Count this packet as received
-		status->packet_rx_success_count++;
-	}
-
-	r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
-	r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
-	r_mavlink_status->packet_rx_drop_count = status->parse_error;
-	status->parse_error = 0;
-	return status->msg_received;
-}
-
-/**
- * @brief Put a bitfield of length 1-32 bit into the buffer
- *
- * @param b the value to add, will be encoded in the bitfield
- * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
- * @param packet_index the position in the packet (the index of the first byte to use)
- * @param bit_index the position in the byte (the index of the first bit to use)
- * @param buffer packet buffer to write into
- * @return new position of the last used byte in the buffer
- */
-MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
-{
-	uint16_t bits_remain = bits;
-	// Transform number into network order
-	int32_t v;
-	uint8_t i_bit_index, i_byte_index, curr_bits_n;
-#if MAVLINK_NEED_BYTE_SWAP
-	union {
-		int32_t i;
-		uint8_t b[4];
-	} bin, bout;
-	bin.i = b;
-	bout.b[0] = bin.b[3];
-	bout.b[1] = bin.b[2];
-	bout.b[2] = bin.b[1];
-	bout.b[3] = bin.b[0];
-	v = bout.i;
-#else
-	v = b;
-#endif
-
-	// buffer in
-	// 01100000 01000000 00000000 11110001
-	// buffer out
-	// 11110001 00000000 01000000 01100000
-
-	// Existing partly filled byte (four free slots)
-	// 0111xxxx
-
-	// Mask n free bits
-	// 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
-	// = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
-
-	// Shift n bits into the right position
-	// out = in >> n;
-
-	// Mask and shift bytes
-	i_bit_index = bit_index;
-	i_byte_index = packet_index;
-	if (bit_index > 0)
-	{
-		// If bits were available at start, they were available
-		// in the byte before the current index
-		i_byte_index--;
-	}
-
-	// While bits have not been packed yet
-	while (bits_remain > 0)
-	{
-		// Bits still have to be packed
-		// there can be more than 8 bits, so
-		// we might have to pack them into more than one byte
-
-		// First pack everything we can into the current 'open' byte
-		//curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
-		//FIXME
-		if (bits_remain <= (uint8_t)(8 - i_bit_index))
-		{
-			// Enough space
-			curr_bits_n = (uint8_t)bits_remain;
-		}
-		else
-		{
-			curr_bits_n = (8 - i_bit_index);
-		}
-		
-		// Pack these n bits into the current byte
-		// Mask out whatever was at that position with ones (xxx11111)
-		buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
-		// Put content to this position, by masking out the non-used part
-		buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
-		
-		// Increment the bit index
-		i_bit_index += curr_bits_n;
-
-		// Now proceed to the next byte, if necessary
-		bits_remain -= curr_bits_n;
-		if (bits_remain > 0)
-		{
-			// Offer another 8 bits / one byte
-			i_byte_index++;
-			i_bit_index = 0;
-		}
-	}
-	
-	*r_bit_index = i_bit_index;
-	// If a partly filled byte is present, mark this as consumed
-	if (i_bit_index != 7) i_byte_index++;
-	return i_byte_index - packet_index;
-}
-
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-// To make MAVLink work on your MCU, define comm_send_ch() if you wish
-// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
-// whole packet at a time
-
-/*
-
-#include "mavlink_types.h"
-
-void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
-{
-    if (chan == MAVLINK_COMM_0)
-    {
-        uart0_transmit(ch);
-    }
-    if (chan == MAVLINK_COMM_1)
-    {
-    	uart1_transmit(ch);
-    }
-}
- */
-
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
-{
-#ifdef MAVLINK_SEND_UART_BYTES
-	/* this is the more efficient approach, if the platform
-	   defines it */
-	MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
-#else
-	/* fallback to one byte at a time */
-	uint16_t i;
-	for (i = 0; i < len; i++) {
-		comm_send_ch(chan, (uint8_t)buf[i]);
-	}
-#endif
-}
-#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-#endif /* _MAVLINK_HELPERS_H_ */
diff --git a/mavlink/include/mavlink/v1.0/mavlink_protobuf_manager.hpp b/mavlink/include/mavlink/v1.0/mavlink_protobuf_manager.hpp
deleted file mode 100644
index fd3ddd026..000000000
--- a/mavlink/include/mavlink/v1.0/mavlink_protobuf_manager.hpp
+++ /dev/null
@@ -1,377 +0,0 @@
-#ifndef MAVLINKPROTOBUFMANAGER_HPP
-#define MAVLINKPROTOBUFMANAGER_HPP
-
-#include <deque>
-#include <google/protobuf/message.h>
-#include <iostream>
-#include <tr1/memory>
-
-#include <checksum.h>
-#include <common/mavlink.h>
-#include <mavlink_types.h>
-#include <pixhawk/pixhawk.pb.h>
-
-namespace mavlink
-{
-
-class ProtobufManager
-{
-public:
-	ProtobufManager()
-	 : mRegisteredTypeCount(0)
-	 , mStreamID(0)
-	 , mVerbose(false)
-	 , kExtendedHeaderSize(MAVLINK_EXTENDED_HEADER_LEN)
-	 , kExtendedPayloadMaxSize(MAVLINK_MAX_EXTENDED_PAYLOAD_LEN)
-	{
-		// register GLOverlay
-		{
-			std::tr1::shared_ptr<px::GLOverlay> msg(new px::GLOverlay);
-			registerType(msg);
-		}
-
-		// register ObstacleList
-		{
-			std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
-			registerType(msg);
-		}
-
-		// register ObstacleMap
-		{
-			std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
-			registerType(msg);
-		}
-
-		// register Path
-		{
-			std::tr1::shared_ptr<px::Path> msg(new px::Path);
-                        registerType(msg);
-		}
-
-		// register PointCloudXYZI
-		{
-			std::tr1::shared_ptr<px::PointCloudXYZI> msg(new px::PointCloudXYZI);
-			registerType(msg);
-		}
-
-		// register PointCloudXYZRGB
-		{
-			std::tr1::shared_ptr<px::PointCloudXYZRGB> msg(new px::PointCloudXYZRGB);
-			registerType(msg);
-		}
-
-		// register RGBDImage
-		{
-			std::tr1::shared_ptr<px::RGBDImage> msg(new px::RGBDImage);
-			registerType(msg);
-		}
-
-		srand(time(NULL));
-		mStreamID = rand() + 1;
-	}
-
-	bool fragmentMessage(uint8_t system_id, uint8_t component_id,
-						 uint8_t target_system, uint8_t target_component,
-						 const google::protobuf::Message& protobuf_msg,
-						 std::vector<mavlink_extended_message_t>& fragments) const
-	{
-		TypeMap::const_iterator it = mTypeMap.find(protobuf_msg.GetTypeName());
-		if (it == mTypeMap.end())
-		{
-			std::cout << "# WARNING: Protobuf message with type "
-					  << protobuf_msg.GetTypeName() << " is not registered."
-					  << std::endl;
-			return false;
-		}
-
-		uint8_t typecode = it->second;
-
-		std::string data = protobuf_msg.SerializeAsString();
-
-		int fragmentCount = (protobuf_msg.ByteSize() + kExtendedPayloadMaxSize - 1) / kExtendedPayloadMaxSize;
-		unsigned int offset = 0;
-
-		for (int i = 0; i < fragmentCount; ++i)
-		{
-			mavlink_extended_message_t fragment;			
-
-			// write extended header data
-			uint8_t* payload = reinterpret_cast<uint8_t*>(fragment.base_msg.payload64);
-			unsigned int length = 0;
-			uint8_t flags = 0;
-
-			if (i < fragmentCount - 1)
-			{
-				length = kExtendedPayloadMaxSize;
-				flags |= 0x1;
-			}
-			else
-			{
-				length = protobuf_msg.ByteSize() - kExtendedPayloadMaxSize * (fragmentCount - 1);
-			}
-
-			memcpy(payload, &target_system, 1);
-			memcpy(payload + 1, &target_component, 1);
-			memcpy(payload + 2, &typecode, 1);
-			memcpy(payload + 3, &length, 4);
-			memcpy(payload + 7, &mStreamID, 2);
-			memcpy(payload + 9, &offset, 4);
-			memcpy(payload + 13, &flags, 1);
-
-			fragment.base_msg.msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE;
-			mavlink_finalize_message(&fragment.base_msg, system_id, component_id, kExtendedHeaderSize, 0);
-			
-			// write extended payload data
-			fragment.extended_payload_len = length;
-			memcpy(fragment.extended_payload, &data[offset], length);
-
-			fragments.push_back(fragment);
-			offset += length;
-		}
-
-		if (mVerbose)
-		{
-			std::cerr << "# INFO: Split extended message with size "
-					  << protobuf_msg.ByteSize() << " into "
-					  << fragmentCount << " fragments." << std::endl;
-		}
-
-		return true;
-	}
-
-	bool cacheFragment(mavlink_extended_message_t& msg)
-	{
-		if (!validFragment(msg))
-		{
-			if (mVerbose)
-			{
-				std::cerr << "# WARNING: Message is not a valid fragment. "
-						  << "Dropping message..." << std::endl;
-			}
-			return false;
-		}
-
-		// read extended header
-		uint8_t* payload = reinterpret_cast<uint8_t*>(msg.base_msg.payload64);
-		uint8_t typecode = 0;
-		unsigned int length = 0;
-		unsigned short streamID = 0;
-		unsigned int offset = 0;
-		uint8_t flags = 0;
-
-		memcpy(&typecode, payload + 2, 1);
-		memcpy(&length, payload + 3, 4);
-		memcpy(&streamID, payload + 7, 2);
-		memcpy(&offset, payload + 9, 4);
-		memcpy(&flags, payload + 13, 1);
-
-		if (typecode >= mTypeMap.size())
-		{
-			std::cout << "# WARNING: Protobuf message with type code "
-					  << static_cast<int>(typecode) << " is not registered." << std::endl;
-			return false;
-		}
-
-		bool reassemble = false;
-
-		FragmentQueue::iterator it = mFragmentQueue.find(streamID);
-		if (it == mFragmentQueue.end())
-		{
-			if (offset == 0)
-			{
-				mFragmentQueue[streamID].push_back(msg);
-
-				if ((flags & 0x1) != 0x1)
-				{
-					reassemble = true;
-				}
-
-				if (mVerbose)
-				{
-					std::cerr << "# INFO: Added fragment to new queue."
-							  << std::endl;
-				}
-			}
-			else
-			{
-				if (mVerbose)
-				{
-					std::cerr << "# WARNING: Message is not a valid fragment. "
-							  << "Dropping message..." << std::endl;
-				}
-			}
-		}
-		else
-		{
-			std::deque<mavlink_extended_message_t>& queue = it->second;
-
-			if (queue.empty())
-			{
-				if (offset == 0)
-				{
-					queue.push_back(msg);
-
-					if ((flags & 0x1) != 0x1)
-					{
-						reassemble = true;
-					}
-				}
-				else
-				{
-					if (mVerbose)
-					{
-						std::cerr << "# WARNING: Message is not a valid fragment. "
-								  << "Dropping message..." << std::endl;
-					}
-				}
-			}
-			else
-			{
-				if (fragmentDataSize(queue.back()) + fragmentOffset(queue.back()) != offset)
-				{
-					if (mVerbose)
-					{
-						std::cerr << "# WARNING: Previous fragment(s) have been lost. "
-								  << "Dropping message and clearing queue..." << std::endl;
-					}
-					queue.clear();
-				}
-				else
-				{
-					queue.push_back(msg);
-
-					if ((flags & 0x1) != 0x1)
-					{
-						reassemble = true;
-					}
-				}
-			}
-		}
-
-		if (reassemble)
-		{
-			std::deque<mavlink_extended_message_t>& queue = mFragmentQueue[streamID];
-
-			std::string data;
-			for (size_t i = 0; i < queue.size(); ++i)
-			{
-				mavlink_extended_message_t& mavlink_msg = queue.at(i);
-
-				data.append(reinterpret_cast<char*>(&mavlink_msg.extended_payload[0]),
-							static_cast<size_t>(mavlink_msg.extended_payload_len));
-			}
-
-			mMessages.at(typecode)->ParseFromString(data);
-
-			mMessageAvailable.at(typecode) = true;
-
-			queue.clear();
-
-			if (mVerbose)
-			{
-				std::cerr << "# INFO: Reassembled fragments for message with typename "
-						  << mMessages.at(typecode)->GetTypeName() << " and size "
-						  << mMessages.at(typecode)->ByteSize()
-						  << "." << std::endl;
-			}
-		}
-
-		return true;
-	}
-
-	bool getMessage(std::tr1::shared_ptr<google::protobuf::Message>& msg)
-	{
-		for (size_t i = 0; i < mMessageAvailable.size(); ++i)
-		{
-			if (mMessageAvailable.at(i))
-			{
-				msg = mMessages.at(i);
-				mMessageAvailable.at(i) = false;
-
-				return true;
-			}
-		}
-
-		return false;
-	}
-
-private:
-	void registerType(const std::tr1::shared_ptr<google::protobuf::Message>& msg)
-	{
-		mTypeMap[msg->GetTypeName()] = mRegisteredTypeCount;
-		++mRegisteredTypeCount;
-		mMessages.push_back(msg);
-		mMessageAvailable.push_back(false);
-	}
-
-	bool validFragment(const mavlink_extended_message_t& msg) const
-	{
-		if (msg.base_msg.magic != MAVLINK_STX ||
-			msg.base_msg.len != kExtendedHeaderSize ||
-			msg.base_msg.msgid != MAVLINK_MSG_ID_EXTENDED_MESSAGE)
-		{
-			return false;
-		}
-
-		uint16_t checksum;
-		checksum = crc_calculate(reinterpret_cast<const uint8_t*>(&msg.base_msg.len), MAVLINK_CORE_HEADER_LEN);
-		crc_accumulate_buffer(&checksum, reinterpret_cast<const char*>(&msg.base_msg.payload64), kExtendedHeaderSize);
-#if MAVLINK_CRC_EXTRA
-		static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
-		crc_accumulate(mavlink_message_crcs[msg.base_msg.msgid], &checksum);
-#endif
-
-		if (mavlink_ck_a(&(msg.base_msg)) != (uint8_t)(checksum & 0xFF) &&
-		    mavlink_ck_b(&(msg.base_msg)) != (uint8_t)(checksum >> 8))
-		{
-			return false;
-		}
-
-		return true;
-	}
-
-	unsigned int fragmentDataSize(const mavlink_extended_message_t& msg) const
-	{
-		const uint8_t* payload = reinterpret_cast<const uint8_t*>(msg.base_msg.payload64);
-
-		return *(reinterpret_cast<const unsigned int*>(payload + 3));
-	}
-
-	unsigned int fragmentOffset(const mavlink_extended_message_t& msg) const
-	{
-		const uint8_t* payload = reinterpret_cast<const uint8_t*>(msg.base_msg.payload64);
-
-		return *(reinterpret_cast<const unsigned int*>(payload + 9));
-	}
-
-	int mRegisteredTypeCount;
-	unsigned short mStreamID;
-	bool mVerbose;
-
-	typedef std::map<std::string, uint8_t> TypeMap;
-	TypeMap mTypeMap;
-	std::vector< std::tr1::shared_ptr<google::protobuf::Message> > mMessages;
-	std::vector<bool> mMessageAvailable;
-
-	typedef std::map<unsigned short, std::deque<mavlink_extended_message_t> > FragmentQueue;
-	FragmentQueue mFragmentQueue;
-
-	const int kExtendedHeaderSize;
-	/**
-	 * Extended header structure
-	 * =========================
-	 *   byte 0 - target_system
-	 *   byte 1 - target_component
-	 *   byte 2 - extended message id (type code)
-	 *   bytes 3-6 - extended payload size in bytes
-	 *   byte 7-8 - stream ID
-	 *   byte 9-12 - fragment offset
-	 *   byte 13 - fragment flags (bit 0 - 1=more fragments, 0=last fragment)
-	 */
-
-	const int kExtendedPayloadMaxSize;
-};
-
-}
-
-#endif
diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h
deleted file mode 100644
index 4019c619e..000000000
--- a/mavlink/include/mavlink/v1.0/mavlink_types.h
+++ /dev/null
@@ -1,161 +0,0 @@
-#ifndef MAVLINK_TYPES_H_
-#define MAVLINK_TYPES_H_
-
-#include <inttypes.h>
-
-#ifndef MAVLINK_MAX_PAYLOAD_LEN
-// it is possible to override this, but be careful!
-#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
-#endif
-
-#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
-#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum
-#define MAVLINK_NUM_CHECKSUM_BYTES 2
-#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
-
-#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
-
-#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
-#define MAVLINK_EXTENDED_HEADER_LEN 14
-
-#if (defined _MSC_VER) | ((defined __APPLE__) & (defined __MACH__)) | (defined __linux__)
-  /* full fledged 32bit++ OS */
-  #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
-#else
-  /* small microcontrollers */
-  #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048
-#endif
-
-#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
-
-typedef struct param_union {
-	union {
-		float param_float;
-		int32_t param_int32;
-		uint32_t param_uint32;
-		int16_t param_int16;
-		uint16_t param_uint16;
-		int8_t param_int8;
-		uint8_t param_uint8;
-		uint8_t bytes[4];
-	};
-	uint8_t type;
-} mavlink_param_union_t;
-
-typedef struct __mavlink_system {
-    uint8_t sysid;   ///< Used by the MAVLink message_xx_send() convenience function
-    uint8_t compid;  ///< Used by the MAVLink message_xx_send() convenience function
-    uint8_t type;    ///< Unused, can be used by user to store the system's type
-    uint8_t state;   ///< Unused, can be used by user to store the system's state
-    uint8_t mode;    ///< Unused, can be used by user to store the system's mode
-    uint32_t nav_mode;    ///< Unused, can be used by user to store the system's navigation mode
-} mavlink_system_t;
-
-typedef struct __mavlink_message {
-	uint16_t checksum; /// sent at end of packet
-	uint8_t magic;   ///< protocol magic marker
-	uint8_t len;     ///< Length of payload
-	uint8_t seq;     ///< Sequence of packet
-	uint8_t sysid;   ///< ID of message sender system/aircraft
-	uint8_t compid;  ///< ID of the message sender component
-	uint8_t msgid;   ///< ID of message in payload
-	uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
-} mavlink_message_t;
-
-
-typedef struct __mavlink_extended_message {
-       mavlink_message_t base_msg;
-       int32_t extended_payload_len;   ///< Length of extended payload if any
-       uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
-} mavlink_extended_message_t;
-
-
-typedef enum {
-	MAVLINK_TYPE_CHAR     = 0,
-	MAVLINK_TYPE_UINT8_T  = 1,
-	MAVLINK_TYPE_INT8_T   = 2,
-	MAVLINK_TYPE_UINT16_T = 3,
-	MAVLINK_TYPE_INT16_T  = 4,
-	MAVLINK_TYPE_UINT32_T = 5,
-	MAVLINK_TYPE_INT32_T  = 6,
-	MAVLINK_TYPE_UINT64_T = 7,
-	MAVLINK_TYPE_INT64_T  = 8,
-	MAVLINK_TYPE_FLOAT    = 9,
-	MAVLINK_TYPE_DOUBLE   = 10
-} mavlink_message_type_t;
-
-#define MAVLINK_MAX_FIELDS 64
-
-typedef struct __mavlink_field_info {
-        const char *name;                 // name of this field
-        const char *print_format;         // printing format hint, or NULL
-        mavlink_message_type_t type;      // type of this field
-        unsigned int array_length;        // if non-zero, field is an array
-        unsigned int wire_offset;         // offset of each field in the payload
-        unsigned int structure_offset;    // offset in a C structure
-} mavlink_field_info_t;
-
-// note that in this structure the order of fields is the order
-// in the XML file, not necessary the wire order
-typedef struct __mavlink_message_info {
-	const char *name;                                      // name of the message
-	unsigned num_fields;                                   // how many fields in this message
-	mavlink_field_info_t fields[MAVLINK_MAX_FIELDS];       // field information
-} mavlink_message_info_t;
-
-#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0])))
-#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0])))
-
-// checksum is immediately after the payload bytes
-#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
-#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
-
-typedef enum {
-    MAVLINK_COMM_0,
-    MAVLINK_COMM_1,
-    MAVLINK_COMM_2,
-    MAVLINK_COMM_3
-} mavlink_channel_t;
-
-/*
- * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
- * of buffers they will use. If more are used, then the result will be
- * a stack overrun
- */
-#ifndef MAVLINK_COMM_NUM_BUFFERS
-#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
-# define MAVLINK_COMM_NUM_BUFFERS 16
-#else
-# define MAVLINK_COMM_NUM_BUFFERS 4
-#endif
-#endif
-
-typedef enum {
-    MAVLINK_PARSE_STATE_UNINIT=0,
-    MAVLINK_PARSE_STATE_IDLE,
-    MAVLINK_PARSE_STATE_GOT_STX,
-    MAVLINK_PARSE_STATE_GOT_SEQ,
-    MAVLINK_PARSE_STATE_GOT_LENGTH,
-    MAVLINK_PARSE_STATE_GOT_SYSID,
-    MAVLINK_PARSE_STATE_GOT_COMPID,
-    MAVLINK_PARSE_STATE_GOT_MSGID,
-    MAVLINK_PARSE_STATE_GOT_PAYLOAD,
-    MAVLINK_PARSE_STATE_GOT_CRC1
-} mavlink_parse_state_t; ///< The state machine for the comm parser
-
-typedef struct __mavlink_status {
-    uint8_t msg_received;               ///< Number of received messages
-    uint8_t buffer_overrun;             ///< Number of buffer overruns
-    uint8_t parse_error;                ///< Number of parse errors
-    mavlink_parse_state_t parse_state;  ///< Parsing state machine
-    uint8_t packet_idx;                 ///< Index in current packet
-    uint8_t current_rx_seq;             ///< Sequence number of last packet received
-    uint8_t current_tx_seq;             ///< Sequence number of last packet sent
-    uint16_t packet_rx_success_count;   ///< Received packets
-    uint16_t packet_rx_drop_count;      ///< Number of packet drops
-} mavlink_status_t;
-
-#define MAVLINK_BIG_ENDIAN 0
-#define MAVLINK_LITTLE_ENDIAN 1
-
-#endif /* MAVLINK_TYPES_H_ */
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink.h
deleted file mode 100644
index f06223370..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from pixhawk.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "pixhawk.h"
-
-#endif // MAVLINK_H
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
deleted file mode 100644
index ef5354d5e..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h
+++ /dev/null
@@ -1,353 +0,0 @@
-// MESSAGE ATTITUDE_CONTROL PACKING
-
-#define MAVLINK_MSG_ID_ATTITUDE_CONTROL 200
-
-typedef struct __mavlink_attitude_control_t
-{
- float roll; ///< roll
- float pitch; ///< pitch
- float yaw; ///< yaw
- float thrust; ///< thrust
- uint8_t target; ///< The system to be controlled
- uint8_t roll_manual; ///< roll control enabled auto:0, manual:1
- uint8_t pitch_manual; ///< pitch auto:0, manual:1
- uint8_t yaw_manual; ///< yaw auto:0, manual:1
- uint8_t thrust_manual; ///< thrust auto:0, manual:1
-} mavlink_attitude_control_t;
-
-#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN 21
-#define MAVLINK_MSG_ID_200_LEN 21
-
-#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC 254
-#define MAVLINK_MSG_ID_200_CRC 254
-
-
-
-#define MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL { \
-	"ATTITUDE_CONTROL", \
-	9, \
-	{  { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_attitude_control_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_control_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_control_t, yaw) }, \
-         { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_control_t, thrust) }, \
-         { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_attitude_control_t, target) }, \
-         { "roll_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_attitude_control_t, roll_manual) }, \
-         { "pitch_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_attitude_control_t, pitch_manual) }, \
-         { "yaw_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_attitude_control_t, yaw_manual) }, \
-         { "thrust_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_attitude_control_t, thrust_manual) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a attitude_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 target The system to be controlled
- * @param roll roll
- * @param pitch pitch
- * @param yaw yaw
- * @param thrust thrust
- * @param roll_manual roll control enabled auto:0, manual:1
- * @param pitch_manual pitch auto:0, manual:1
- * @param yaw_manual yaw auto:0, manual:1
- * @param thrust_manual thrust auto:0, manual:1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
-#else
-	mavlink_attitude_control_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a attitude_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 target The system to be controlled
- * @param roll roll
- * @param pitch pitch
- * @param yaw yaw
- * @param thrust thrust
- * @param roll_manual roll control enabled auto:0, manual:1
- * @param pitch_manual pitch auto:0, manual:1
- * @param yaw_manual yaw auto:0, manual:1
- * @param thrust_manual thrust auto:0, manual:1
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
-#else
-	mavlink_attitude_control_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a attitude_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 attitude_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control)
-{
-	return mavlink_msg_attitude_control_pack(system_id, component_id, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual);
-}
-
-/**
- * @brief Encode a attitude_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 attitude_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_attitude_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control)
-{
-	return mavlink_msg_attitude_control_pack_chan(system_id, component_id, chan, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual);
-}
-
-/**
- * @brief Send a attitude_control message
- * @param chan MAVLink channel to send the message
- *
- * @param target The system to be controlled
- * @param roll roll
- * @param pitch pitch
- * @param yaw yaw
- * @param thrust thrust
- * @param roll_manual roll control enabled auto:0, manual:1
- * @param pitch_manual pitch auto:0, manual:1
- * @param yaw_manual yaw auto:0, manual:1
- * @param thrust_manual thrust auto:0, manual:1
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_attitude_control_send(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[MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN];
-	_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;
-	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
-
-// MESSAGE ATTITUDE_CONTROL UNPACKING
-
-
-/**
- * @brief Get field target from attitude_control message
- *
- * @return The system to be controlled
- */
-static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field roll from attitude_control message
- *
- * @return roll
- */
-static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field pitch from attitude_control message
- *
- * @return pitch
- */
-static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field yaw from attitude_control message
- *
- * @return yaw
- */
-static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field thrust from attitude_control message
- *
- * @return thrust
- */
-static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field roll_manual from attitude_control message
- *
- * @return roll control enabled auto:0, manual:1
- */
-static inline uint8_t mavlink_msg_attitude_control_get_roll_manual(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  17);
-}
-
-/**
- * @brief Get field pitch_manual from attitude_control message
- *
- * @return pitch auto:0, manual:1
- */
-static inline uint8_t mavlink_msg_attitude_control_get_pitch_manual(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  18);
-}
-
-/**
- * @brief Get field yaw_manual from attitude_control message
- *
- * @return yaw auto:0, manual:1
- */
-static inline uint8_t mavlink_msg_attitude_control_get_yaw_manual(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  19);
-}
-
-/**
- * @brief Get field thrust_manual from attitude_control message
- *
- * @return thrust auto:0, manual:1
- */
-static inline uint8_t mavlink_msg_attitude_control_get_thrust_manual(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  20);
-}
-
-/**
- * @brief Decode a attitude_control message into a struct
- *
- * @param msg The message to decode
- * @param attitude_control C-struct to decode the message contents into
- */
-static inline void mavlink_msg_attitude_control_decode(const mavlink_message_t* msg, mavlink_attitude_control_t* attitude_control)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	attitude_control->roll = mavlink_msg_attitude_control_get_roll(msg);
-	attitude_control->pitch = mavlink_msg_attitude_control_get_pitch(msg);
-	attitude_control->yaw = mavlink_msg_attitude_control_get_yaw(msg);
-	attitude_control->thrust = mavlink_msg_attitude_control_get_thrust(msg);
-	attitude_control->target = mavlink_msg_attitude_control_get_target(msg);
-	attitude_control->roll_manual = mavlink_msg_attitude_control_get_roll_manual(msg);
-	attitude_control->pitch_manual = mavlink_msg_attitude_control_get_pitch_manual(msg);
-	attitude_control->yaw_manual = mavlink_msg_attitude_control_get_yaw_manual(msg);
-	attitude_control->thrust_manual = mavlink_msg_attitude_control_get_thrust_manual(msg);
-#else
-	memcpy(attitude_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
-#endif
-}
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
deleted file mode 100644
index d41093792..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h
+++ /dev/null
@@ -1,325 +0,0 @@
-// MESSAGE BRIEF_FEATURE PACKING
-
-#define MAVLINK_MSG_ID_BRIEF_FEATURE 195
-
-typedef struct __mavlink_brief_feature_t
-{
- float x; ///< x position in m
- float y; ///< y position in m
- float z; ///< z position in m
- float response; ///< Harris operator response at this location
- uint16_t size; ///< Size in pixels
- uint16_t orientation; ///< Orientation
- uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true
- uint8_t descriptor[32]; ///< Descriptor
-} mavlink_brief_feature_t;
-
-#define MAVLINK_MSG_ID_BRIEF_FEATURE_LEN 53
-#define MAVLINK_MSG_ID_195_LEN 53
-
-#define MAVLINK_MSG_ID_BRIEF_FEATURE_CRC 88
-#define MAVLINK_MSG_ID_195_CRC 88
-
-#define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32
-
-#define MAVLINK_MESSAGE_INFO_BRIEF_FEATURE { \
-	"BRIEF_FEATURE", \
-	8, \
-	{  { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_brief_feature_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_brief_feature_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_brief_feature_t, z) }, \
-         { "response", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_brief_feature_t, response) }, \
-         { "size", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_brief_feature_t, size) }, \
-         { "orientation", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_brief_feature_t, orientation) }, \
-         { "orientation_assignment", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_brief_feature_t, orientation_assignment) }, \
-         { "descriptor", NULL, MAVLINK_TYPE_UINT8_T, 32, 21, offsetof(mavlink_brief_feature_t, descriptor) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a brief_feature 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 x x position in m
- * @param y y position in m
- * @param z z position in m
- * @param orientation_assignment Orientation assignment 0: false, 1:true
- * @param size Size in pixels
- * @param orientation Orientation
- * @param descriptor Descriptor
- * @param response Harris operator response at this location
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
-#else
-	mavlink_brief_feature_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a brief_feature 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 x x position in m
- * @param y y position in m
- * @param z z position in m
- * @param orientation_assignment Orientation assignment 0: false, 1:true
- * @param size Size in pixels
- * @param orientation Orientation
- * @param descriptor Descriptor
- * @param response Harris operator response at this location
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
-#else
-	mavlink_brief_feature_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a brief_feature 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 brief_feature C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature)
-{
-	return mavlink_msg_brief_feature_pack(system_id, component_id, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response);
-}
-
-/**
- * @brief Encode a brief_feature 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 brief_feature C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_brief_feature_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature)
-{
-	return mavlink_msg_brief_feature_pack_chan(system_id, component_id, chan, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response);
-}
-
-/**
- * @brief Send a brief_feature message
- * @param chan MAVLink channel to send the message
- *
- * @param x x position in m
- * @param y y position in m
- * @param z z position in m
- * @param orientation_assignment Orientation assignment 0: false, 1:true
- * @param size Size in pixels
- * @param orientation Orientation
- * @param descriptor Descriptor
- * @param response Harris operator response at this location
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_brief_feature_send(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[MAVLINK_MSG_ID_BRIEF_FEATURE_LEN];
-	_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;
-	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
-
-// MESSAGE BRIEF_FEATURE UNPACKING
-
-
-/**
- * @brief Get field x from brief_feature message
- *
- * @return x position in m
- */
-static inline float mavlink_msg_brief_feature_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field y from brief_feature message
- *
- * @return y position in m
- */
-static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field z from brief_feature message
- *
- * @return z position in m
- */
-static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field orientation_assignment from brief_feature message
- *
- * @return Orientation assignment 0: false, 1:true
- */
-static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  20);
-}
-
-/**
- * @brief Get field size from brief_feature message
- *
- * @return Size in pixels
- */
-static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field orientation from brief_feature message
- *
- * @return Orientation
- */
-static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  18);
-}
-
-/**
- * @brief Get field descriptor from brief_feature message
- *
- * @return Descriptor
- */
-static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t *descriptor)
-{
-	return _MAV_RETURN_uint8_t_array(msg, descriptor, 32,  21);
-}
-
-/**
- * @brief Get field response from brief_feature message
- *
- * @return Harris operator response at this location
- */
-static inline float mavlink_msg_brief_feature_get_response(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Decode a brief_feature message into a struct
- *
- * @param msg The message to decode
- * @param brief_feature C-struct to decode the message contents into
- */
-static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* msg, mavlink_brief_feature_t* brief_feature)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	brief_feature->x = mavlink_msg_brief_feature_get_x(msg);
-	brief_feature->y = mavlink_msg_brief_feature_get_y(msg);
-	brief_feature->z = mavlink_msg_brief_feature_get_z(msg);
-	brief_feature->response = mavlink_msg_brief_feature_get_response(msg);
-	brief_feature->size = mavlink_msg_brief_feature_get_size(msg);
-	brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg);
-	brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg);
-	mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor);
-#else
-	memcpy(brief_feature, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
-#endif
-}
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
deleted file mode 100644
index ae4db825d..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h
+++ /dev/null
@@ -1,661 +0,0 @@
-// MESSAGE IMAGE_AVAILABLE PACKING
-
-#define MAVLINK_MSG_ID_IMAGE_AVAILABLE 154
-
-typedef struct __mavlink_image_available_t
-{
- uint64_t cam_id; ///< Camera id
- uint64_t timestamp; ///< Timestamp
- uint64_t valid_until; ///< Until which timestamp this buffer will stay valid
- uint32_t img_seq; ///< The image sequence number
- uint32_t img_buf_index; ///< Position of the image in the buffer, starts with 0
- uint32_t key; ///< Shared memory area key
- uint32_t exposure; ///< Exposure time, in microseconds
- float gain; ///< Camera gain
- float roll; ///< Roll angle in rad
- float pitch; ///< Pitch angle in rad
- float yaw; ///< Yaw angle in rad
- float local_z; ///< Local frame Z coordinate (height over ground)
- float lat; ///< GPS X coordinate
- float lon; ///< GPS Y coordinate
- float alt; ///< Global frame altitude
- float ground_x; ///< Ground truth X
- float ground_y; ///< Ground truth Y
- float ground_z; ///< Ground truth Z
- uint16_t width; ///< Image width
- uint16_t height; ///< Image height
- uint16_t depth; ///< Image depth
- uint8_t cam_no; ///< Camera # (starts with 0)
- uint8_t channels; ///< Image channels
-} mavlink_image_available_t;
-
-#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN 92
-#define MAVLINK_MSG_ID_154_LEN 92
-
-#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC 224
-#define MAVLINK_MSG_ID_154_CRC 224
-
-
-
-#define MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE { \
-	"IMAGE_AVAILABLE", \
-	23, \
-	{  { "cam_id", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_image_available_t, cam_id) }, \
-         { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_image_available_t, timestamp) }, \
-         { "valid_until", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_image_available_t, valid_until) }, \
-         { "img_seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_image_available_t, img_seq) }, \
-         { "img_buf_index", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_image_available_t, img_buf_index) }, \
-         { "key", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_image_available_t, key) }, \
-         { "exposure", NULL, MAVLINK_TYPE_UINT32_T, 0, 36, offsetof(mavlink_image_available_t, exposure) }, \
-         { "gain", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_image_available_t, gain) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_image_available_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_image_available_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_image_available_t, yaw) }, \
-         { "local_z", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_image_available_t, local_z) }, \
-         { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_image_available_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_image_available_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_image_available_t, alt) }, \
-         { "ground_x", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_image_available_t, ground_x) }, \
-         { "ground_y", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_image_available_t, ground_y) }, \
-         { "ground_z", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_image_available_t, ground_z) }, \
-         { "width", NULL, MAVLINK_TYPE_UINT16_T, 0, 84, offsetof(mavlink_image_available_t, width) }, \
-         { "height", NULL, MAVLINK_TYPE_UINT16_T, 0, 86, offsetof(mavlink_image_available_t, height) }, \
-         { "depth", NULL, MAVLINK_TYPE_UINT16_T, 0, 88, offsetof(mavlink_image_available_t, depth) }, \
-         { "cam_no", NULL, MAVLINK_TYPE_UINT8_T, 0, 90, offsetof(mavlink_image_available_t, cam_no) }, \
-         { "channels", NULL, MAVLINK_TYPE_UINT8_T, 0, 91, offsetof(mavlink_image_available_t, channels) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a image_available 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 cam_id Camera id
- * @param cam_no Camera # (starts with 0)
- * @param timestamp Timestamp
- * @param valid_until Until which timestamp this buffer will stay valid
- * @param img_seq The image sequence number
- * @param img_buf_index Position of the image in the buffer, starts with 0
- * @param width Image width
- * @param height Image height
- * @param depth Image depth
- * @param channels Image channels
- * @param key Shared memory area key
- * @param exposure Exposure time, in microseconds
- * @param gain Camera gain
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @param local_z Local frame Z coordinate (height over ground)
- * @param lat GPS X coordinate
- * @param lon GPS Y coordinate
- * @param alt Global frame altitude
- * @param ground_x Ground truth X
- * @param ground_y Ground truth Y
- * @param ground_z Ground truth Z
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
-#else
-	mavlink_image_available_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a image_available 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 cam_id Camera id
- * @param cam_no Camera # (starts with 0)
- * @param timestamp Timestamp
- * @param valid_until Until which timestamp this buffer will stay valid
- * @param img_seq The image sequence number
- * @param img_buf_index Position of the image in the buffer, starts with 0
- * @param width Image width
- * @param height Image height
- * @param depth Image depth
- * @param channels Image channels
- * @param key Shared memory area key
- * @param exposure Exposure time, in microseconds
- * @param gain Camera gain
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @param local_z Local frame Z coordinate (height over ground)
- * @param lat GPS X coordinate
- * @param lon GPS Y coordinate
- * @param alt Global frame altitude
- * @param ground_x Ground truth X
- * @param ground_y Ground truth Y
- * @param ground_z Ground truth Z
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
-#else
-	mavlink_image_available_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a image_available 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 image_available C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_available_t* image_available)
-{
-	return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z);
-}
-
-/**
- * @brief Encode a image_available 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 image_available C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_image_available_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_available_t* image_available)
-{
-	return mavlink_msg_image_available_pack_chan(system_id, component_id, chan, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z);
-}
-
-/**
- * @brief Send a image_available message
- * @param chan MAVLink channel to send the message
- *
- * @param cam_id Camera id
- * @param cam_no Camera # (starts with 0)
- * @param timestamp Timestamp
- * @param valid_until Until which timestamp this buffer will stay valid
- * @param img_seq The image sequence number
- * @param img_buf_index Position of the image in the buffer, starts with 0
- * @param width Image width
- * @param height Image height
- * @param depth Image depth
- * @param channels Image channels
- * @param key Shared memory area key
- * @param exposure Exposure time, in microseconds
- * @param gain Camera gain
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @param local_z Local frame Z coordinate (height over ground)
- * @param lat GPS X coordinate
- * @param lon GPS Y coordinate
- * @param alt Global frame altitude
- * @param ground_x Ground truth X
- * @param ground_y Ground truth Y
- * @param ground_z Ground truth Z
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_image_available_send(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[MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN];
-	_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;
-	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
-
-// MESSAGE IMAGE_AVAILABLE UNPACKING
-
-
-/**
- * @brief Get field cam_id from image_available message
- *
- * @return Camera id
- */
-static inline uint64_t mavlink_msg_image_available_get_cam_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field cam_no from image_available message
- *
- * @return Camera # (starts with 0)
- */
-static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  90);
-}
-
-/**
- * @brief Get field timestamp from image_available message
- *
- * @return Timestamp
- */
-static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  8);
-}
-
-/**
- * @brief Get field valid_until from image_available message
- *
- * @return Until which timestamp this buffer will stay valid
- */
-static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  16);
-}
-
-/**
- * @brief Get field img_seq from image_available message
- *
- * @return The image sequence number
- */
-static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  24);
-}
-
-/**
- * @brief Get field img_buf_index from image_available message
- *
- * @return Position of the image in the buffer, starts with 0
- */
-static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  28);
-}
-
-/**
- * @brief Get field width from image_available message
- *
- * @return Image width
- */
-static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  84);
-}
-
-/**
- * @brief Get field height from image_available message
- *
- * @return Image height
- */
-static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  86);
-}
-
-/**
- * @brief Get field depth from image_available message
- *
- * @return Image depth
- */
-static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  88);
-}
-
-/**
- * @brief Get field channels from image_available message
- *
- * @return Image channels
- */
-static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  91);
-}
-
-/**
- * @brief Get field key from image_available message
- *
- * @return Shared memory area key
- */
-static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  32);
-}
-
-/**
- * @brief Get field exposure from image_available message
- *
- * @return Exposure time, in microseconds
- */
-static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  36);
-}
-
-/**
- * @brief Get field gain from image_available message
- *
- * @return Camera gain
- */
-static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  40);
-}
-
-/**
- * @brief Get field roll from image_available message
- *
- * @return Roll angle in rad
- */
-static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  44);
-}
-
-/**
- * @brief Get field pitch from image_available message
- *
- * @return Pitch angle in rad
- */
-static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  48);
-}
-
-/**
- * @brief Get field yaw from image_available message
- *
- * @return Yaw angle in rad
- */
-static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  52);
-}
-
-/**
- * @brief Get field local_z from image_available message
- *
- * @return Local frame Z coordinate (height over ground)
- */
-static inline float mavlink_msg_image_available_get_local_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  56);
-}
-
-/**
- * @brief Get field lat from image_available message
- *
- * @return GPS X coordinate
- */
-static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  60);
-}
-
-/**
- * @brief Get field lon from image_available message
- *
- * @return GPS Y coordinate
- */
-static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  64);
-}
-
-/**
- * @brief Get field alt from image_available message
- *
- * @return Global frame altitude
- */
-static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  68);
-}
-
-/**
- * @brief Get field ground_x from image_available message
- *
- * @return Ground truth X
- */
-static inline float mavlink_msg_image_available_get_ground_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  72);
-}
-
-/**
- * @brief Get field ground_y from image_available message
- *
- * @return Ground truth Y
- */
-static inline float mavlink_msg_image_available_get_ground_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  76);
-}
-
-/**
- * @brief Get field ground_z from image_available message
- *
- * @return Ground truth Z
- */
-static inline float mavlink_msg_image_available_get_ground_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  80);
-}
-
-/**
- * @brief Decode a image_available message into a struct
- *
- * @param msg The message to decode
- * @param image_available C-struct to decode the message contents into
- */
-static inline void mavlink_msg_image_available_decode(const mavlink_message_t* msg, mavlink_image_available_t* image_available)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	image_available->cam_id = mavlink_msg_image_available_get_cam_id(msg);
-	image_available->timestamp = mavlink_msg_image_available_get_timestamp(msg);
-	image_available->valid_until = mavlink_msg_image_available_get_valid_until(msg);
-	image_available->img_seq = mavlink_msg_image_available_get_img_seq(msg);
-	image_available->img_buf_index = mavlink_msg_image_available_get_img_buf_index(msg);
-	image_available->key = mavlink_msg_image_available_get_key(msg);
-	image_available->exposure = mavlink_msg_image_available_get_exposure(msg);
-	image_available->gain = mavlink_msg_image_available_get_gain(msg);
-	image_available->roll = mavlink_msg_image_available_get_roll(msg);
-	image_available->pitch = mavlink_msg_image_available_get_pitch(msg);
-	image_available->yaw = mavlink_msg_image_available_get_yaw(msg);
-	image_available->local_z = mavlink_msg_image_available_get_local_z(msg);
-	image_available->lat = mavlink_msg_image_available_get_lat(msg);
-	image_available->lon = mavlink_msg_image_available_get_lon(msg);
-	image_available->alt = mavlink_msg_image_available_get_alt(msg);
-	image_available->ground_x = mavlink_msg_image_available_get_ground_x(msg);
-	image_available->ground_y = mavlink_msg_image_available_get_ground_y(msg);
-	image_available->ground_z = mavlink_msg_image_available_get_ground_z(msg);
-	image_available->width = mavlink_msg_image_available_get_width(msg);
-	image_available->height = mavlink_msg_image_available_get_height(msg);
-	image_available->depth = mavlink_msg_image_available_get_depth(msg);
-	image_available->cam_no = mavlink_msg_image_available_get_cam_no(msg);
-	image_available->channels = mavlink_msg_image_available_get_channels(msg);
-#else
-	memcpy(image_available, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
-#endif
-}
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
deleted file mode 100644
index 664574be9..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h
+++ /dev/null
@@ -1,177 +0,0 @@
-// MESSAGE IMAGE_TRIGGER_CONTROL PACKING
-
-#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 153
-
-typedef struct __mavlink_image_trigger_control_t
-{
- uint8_t enable; ///< 0 to disable, 1 to enable
-} mavlink_image_trigger_control_t;
-
-#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1
-#define MAVLINK_MSG_ID_153_LEN 1
-
-#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC 95
-#define MAVLINK_MSG_ID_153_CRC 95
-
-
-
-#define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL { \
-	"IMAGE_TRIGGER_CONTROL", \
-	1, \
-	{  { "enable", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_image_trigger_control_t, enable) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a image_trigger_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 enable 0 to disable, 1 to enable
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t enable)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN];
-	_mav_put_uint8_t(buf, 0, enable);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
-#else
-	mavlink_image_trigger_control_t packet;
-	packet.enable = enable;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a image_trigger_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 enable 0 to disable, 1 to enable
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t enable)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN];
-	_mav_put_uint8_t(buf, 0, enable);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
-#else
-	mavlink_image_trigger_control_t packet;
-	packet.enable = enable;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a image_trigger_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 image_trigger_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control)
-{
-	return mavlink_msg_image_trigger_control_pack(system_id, component_id, msg, image_trigger_control->enable);
-}
-
-/**
- * @brief Encode a image_trigger_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 image_trigger_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_image_trigger_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control)
-{
-	return mavlink_msg_image_trigger_control_pack_chan(system_id, component_id, chan, msg, image_trigger_control->enable);
-}
-
-/**
- * @brief Send a image_trigger_control message
- * @param chan MAVLink channel to send the message
- *
- * @param enable 0 to disable, 1 to enable
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN];
-	_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;
-	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
-
-// MESSAGE IMAGE_TRIGGER_CONTROL UNPACKING
-
-
-/**
- * @brief Get field enable from image_trigger_control message
- *
- * @return 0 to disable, 1 to enable
- */
-static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Decode a image_trigger_control message into a struct
- *
- * @param msg The message to decode
- * @param image_trigger_control C-struct to decode the message contents into
- */
-static inline void mavlink_msg_image_trigger_control_decode(const mavlink_message_t* msg, mavlink_image_trigger_control_t* image_trigger_control)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg);
-#else
-	memcpy(image_trigger_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
-#endif
-}
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
deleted file mode 100644
index f3a2243af..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h
+++ /dev/null
@@ -1,419 +0,0 @@
-// MESSAGE IMAGE_TRIGGERED PACKING
-
-#define MAVLINK_MSG_ID_IMAGE_TRIGGERED 152
-
-typedef struct __mavlink_image_triggered_t
-{
- uint64_t timestamp; ///< Timestamp
- uint32_t seq; ///< IMU seq
- float roll; ///< Roll angle in rad
- float pitch; ///< Pitch angle in rad
- float yaw; ///< Yaw angle in rad
- float local_z; ///< Local frame Z coordinate (height over ground)
- float lat; ///< GPS X coordinate
- float lon; ///< GPS Y coordinate
- float alt; ///< Global frame altitude
- float ground_x; ///< Ground truth X
- float ground_y; ///< Ground truth Y
- float ground_z; ///< Ground truth Z
-} mavlink_image_triggered_t;
-
-#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN 52
-#define MAVLINK_MSG_ID_152_LEN 52
-
-#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC 86
-#define MAVLINK_MSG_ID_152_CRC 86
-
-
-
-#define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED { \
-	"IMAGE_TRIGGERED", \
-	12, \
-	{  { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_image_triggered_t, timestamp) }, \
-         { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_image_triggered_t, seq) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_image_triggered_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_image_triggered_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_image_triggered_t, yaw) }, \
-         { "local_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_image_triggered_t, local_z) }, \
-         { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_image_triggered_t, lat) }, \
-         { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_image_triggered_t, lon) }, \
-         { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_image_triggered_t, alt) }, \
-         { "ground_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_image_triggered_t, ground_x) }, \
-         { "ground_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_image_triggered_t, ground_y) }, \
-         { "ground_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_image_triggered_t, ground_z) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a image_triggered 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 timestamp Timestamp
- * @param seq IMU seq
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @param local_z Local frame Z coordinate (height over ground)
- * @param lat GPS X coordinate
- * @param lon GPS Y coordinate
- * @param alt Global frame altitude
- * @param ground_x Ground truth X
- * @param ground_y Ground truth Y
- * @param ground_z Ground truth Z
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
-#else
-	mavlink_image_triggered_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
-#endif
-}
-
-/**
- * @brief Pack a image_triggered 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 timestamp Timestamp
- * @param seq IMU seq
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @param local_z Local frame Z coordinate (height over ground)
- * @param lat GPS X coordinate
- * @param lon GPS Y coordinate
- * @param alt Global frame altitude
- * @param ground_x Ground truth X
- * @param ground_y Ground truth Y
- * @param ground_z Ground truth Z
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
-#else
-	mavlink_image_triggered_t packet;
-	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;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
-#endif
-}
-
-/**
- * @brief Encode a image_triggered 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 image_triggered C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered)
-{
-	return mavlink_msg_image_triggered_pack(system_id, component_id, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z);
-}
-
-/**
- * @brief Encode a image_triggered 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 image_triggered C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_image_triggered_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered)
-{
-	return mavlink_msg_image_triggered_pack_chan(system_id, component_id, chan, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z);
-}
-
-/**
- * @brief Send a image_triggered message
- * @param chan MAVLink channel to send the message
- *
- * @param timestamp Timestamp
- * @param seq IMU seq
- * @param roll Roll angle in rad
- * @param pitch Pitch angle in rad
- * @param yaw Yaw angle in rad
- * @param local_z Local frame Z coordinate (height over ground)
- * @param lat GPS X coordinate
- * @param lon GPS Y coordinate
- * @param alt Global frame altitude
- * @param ground_x Ground truth X
- * @param ground_y Ground truth Y
- * @param ground_z Ground truth Z
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_image_triggered_send(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[MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN];
-	_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;
-	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
-
-// MESSAGE IMAGE_TRIGGERED UNPACKING
-
-
-/**
- * @brief Get field timestamp from image_triggered message
- *
- * @return Timestamp
- */
-static inline uint64_t mavlink_msg_image_triggered_get_timestamp(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field seq from image_triggered message
- *
- * @return IMU seq
- */
-static inline uint32_t mavlink_msg_image_triggered_get_seq(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  8);
-}
-
-/**
- * @brief Get field roll from image_triggered message
- *
- * @return Roll angle in rad
- */
-static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field pitch from image_triggered message
- *
- * @return Pitch angle in rad
- */
-static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field yaw from image_triggered message
- *
- * @return Yaw angle in rad
- */
-static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field local_z from image_triggered message
- *
- * @return Local frame Z coordinate (height over ground)
- */
-static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field lat from image_triggered message
- *
- * @return GPS X coordinate
- */
-static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field lon from image_triggered message
- *
- * @return GPS Y coordinate
- */
-static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field alt from image_triggered message
- *
- * @return Global frame altitude
- */
-static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  36);
-}
-
-/**
- * @brief Get field ground_x from image_triggered message
- *
- * @return Ground truth X
- */
-static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  40);
-}
-
-/**
- * @brief Get field ground_y from image_triggered message
- *
- * @return Ground truth Y
- */
-static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  44);
-}
-
-/**
- * @brief Get field ground_z from image_triggered message
- *
- * @return Ground truth Z
- */
-static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  48);
-}
-
-/**
- * @brief Decode a image_triggered message into a struct
- *
- * @param msg The message to decode
- * @param image_triggered C-struct to decode the message contents into
- */
-static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* msg, mavlink_image_triggered_t* image_triggered)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	image_triggered->timestamp = mavlink_msg_image_triggered_get_timestamp(msg);
-	image_triggered->seq = mavlink_msg_image_triggered_get_seq(msg);
-	image_triggered->roll = mavlink_msg_image_triggered_get_roll(msg);
-	image_triggered->pitch = mavlink_msg_image_triggered_get_pitch(msg);
-	image_triggered->yaw = mavlink_msg_image_triggered_get_yaw(msg);
-	image_triggered->local_z = mavlink_msg_image_triggered_get_local_z(msg);
-	image_triggered->lat = mavlink_msg_image_triggered_get_lat(msg);
-	image_triggered->lon = mavlink_msg_image_triggered_get_lon(msg);
-	image_triggered->alt = mavlink_msg_image_triggered_get_alt(msg);
-	image_triggered->ground_x = mavlink_msg_image_triggered_get_ground_x(msg);
-	image_triggered->ground_y = mavlink_msg_image_triggered_get_ground_y(msg);
-	image_triggered->ground_z = mavlink_msg_image_triggered_get_ground_z(msg);
-#else
-	memcpy(image_triggered, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h
deleted file mode 100644
index 6bdcceaf9..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE MARKER PACKING
-
-#define MAVLINK_MSG_ID_MARKER 171
-
-typedef struct __mavlink_marker_t
-{
- float x; ///< x position
- float y; ///< y position
- float z; ///< z position
- float roll; ///< roll orientation
- float pitch; ///< pitch orientation
- float yaw; ///< yaw orientation
- uint16_t id; ///< ID
-} mavlink_marker_t;
-
-#define MAVLINK_MSG_ID_MARKER_LEN 26
-#define MAVLINK_MSG_ID_171_LEN 26
-
-#define MAVLINK_MSG_ID_MARKER_CRC 249
-#define MAVLINK_MSG_ID_171_CRC 249
-
-
-
-#define MAVLINK_MESSAGE_INFO_MARKER { \
-	"MARKER", \
-	7, \
-	{  { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_marker_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_marker_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_marker_t, z) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_marker_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_marker_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_marker_t, yaw) }, \
-         { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_marker_t, id) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a marker 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 id ID
- * @param x x position
- * @param y y position
- * @param z z position
- * @param roll roll orientation
- * @param pitch pitch orientation
- * @param yaw yaw orientation
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_MARKER_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MARKER_LEN);
-#else
-	mavlink_marker_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.id = id;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MARKER_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MARKER;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN);
-#endif
-}
-
-/**
- * @brief Pack a marker 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 id ID
- * @param x x position
- * @param y y position
- * @param z z position
- * @param roll roll orientation
- * @param pitch pitch orientation
- * @param yaw yaw orientation
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_MARKER_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MARKER_LEN);
-#else
-	mavlink_marker_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.id = id;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MARKER_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MARKER;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN);
-#endif
-}
-
-/**
- * @brief Encode a marker 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 marker C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_marker_t* marker)
-{
-	return mavlink_msg_marker_pack(system_id, component_id, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw);
-}
-
-/**
- * @brief Encode a marker 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 marker C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_marker_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_marker_t* marker)
-{
-	return mavlink_msg_marker_pack_chan(system_id, component_id, chan, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw);
-}
-
-/**
- * @brief Send a marker message
- * @param chan MAVLink channel to send the message
- *
- * @param id ID
- * @param x x position
- * @param y y position
- * @param z z position
- * @param roll roll orientation
- * @param pitch pitch orientation
- * @param yaw yaw orientation
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_marker_send(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[MAVLINK_MSG_ID_MARKER_LEN];
-	_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;
-	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
-
-// MESSAGE MARKER UNPACKING
-
-
-/**
- * @brief Get field id from marker message
- *
- * @return ID
- */
-static inline uint16_t mavlink_msg_marker_get_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  24);
-}
-
-/**
- * @brief Get field x from marker message
- *
- * @return x position
- */
-static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field y from marker message
- *
- * @return y position
- */
-static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field z from marker message
- *
- * @return z position
- */
-static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field roll from marker message
- *
- * @return roll orientation
- */
-static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field pitch from marker message
- *
- * @return pitch orientation
- */
-static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field yaw from marker message
- *
- * @return yaw orientation
- */
-static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Decode a marker message into a struct
- *
- * @param msg The message to decode
- * @param marker C-struct to decode the message contents into
- */
-static inline void mavlink_msg_marker_decode(const mavlink_message_t* msg, mavlink_marker_t* marker)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	marker->x = mavlink_msg_marker_get_x(msg);
-	marker->y = mavlink_msg_marker_get_y(msg);
-	marker->z = mavlink_msg_marker_get_z(msg);
-	marker->roll = mavlink_msg_marker_get_roll(msg);
-	marker->pitch = mavlink_msg_marker_get_pitch(msg);
-	marker->yaw = mavlink_msg_marker_get_yaw(msg);
-	marker->id = mavlink_msg_marker_get_id(msg);
-#else
-	memcpy(marker, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MARKER_LEN);
-#endif
-}
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
deleted file mode 100644
index aa7b827a3..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h
+++ /dev/null
@@ -1,237 +0,0 @@
-// MESSAGE PATTERN_DETECTED PACKING
-
-#define MAVLINK_MSG_ID_PATTERN_DETECTED 190
-
-typedef struct __mavlink_pattern_detected_t
-{
- float confidence; ///< Confidence of detection
- uint8_t type; ///< 0: Pattern, 1: Letter
- char file[100]; ///< Pattern file name
- uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes
-} mavlink_pattern_detected_t;
-
-#define MAVLINK_MSG_ID_PATTERN_DETECTED_LEN 106
-#define MAVLINK_MSG_ID_190_LEN 106
-
-#define MAVLINK_MSG_ID_PATTERN_DETECTED_CRC 90
-#define MAVLINK_MSG_ID_190_CRC 90
-
-#define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100
-
-#define MAVLINK_MESSAGE_INFO_PATTERN_DETECTED { \
-	"PATTERN_DETECTED", \
-	4, \
-	{  { "confidence", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pattern_detected_t, confidence) }, \
-         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_pattern_detected_t, type) }, \
-         { "file", NULL, MAVLINK_TYPE_CHAR, 100, 5, offsetof(mavlink_pattern_detected_t, file) }, \
-         { "detected", NULL, MAVLINK_TYPE_UINT8_T, 0, 105, offsetof(mavlink_pattern_detected_t, detected) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a pattern_detected 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 type 0: Pattern, 1: Letter
- * @param confidence Confidence of detection
- * @param file Pattern file name
- * @param detected Accepted as true detection, 0 no, 1 yes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, float confidence, const char *file, uint8_t detected)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
-#else
-	mavlink_pattern_detected_t packet;
-	packet.confidence = confidence;
-	packet.type = type;
-	packet.detected = detected;
-	mav_array_memcpy(packet.file, file, sizeof(char)*100);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
-#endif
-}
-
-/**
- * @brief Pack a pattern_detected 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 type 0: Pattern, 1: Letter
- * @param confidence Confidence of detection
- * @param file Pattern file name
- * @param detected Accepted as true detection, 0 no, 1 yes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,float confidence,const char *file,uint8_t detected)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
-#else
-	mavlink_pattern_detected_t packet;
-	packet.confidence = confidence;
-	packet.type = type;
-	packet.detected = detected;
-	mav_array_memcpy(packet.file, file, sizeof(char)*100);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
-#endif
-}
-
-/**
- * @brief Encode a pattern_detected 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 pattern_detected C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected)
-{
-	return mavlink_msg_pattern_detected_pack(system_id, component_id, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected);
-}
-
-/**
- * @brief Encode a pattern_detected 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 pattern_detected C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_pattern_detected_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected)
-{
-	return mavlink_msg_pattern_detected_pack_chan(system_id, component_id, chan, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected);
-}
-
-/**
- * @brief Send a pattern_detected message
- * @param chan MAVLink channel to send the message
- *
- * @param type 0: Pattern, 1: Letter
- * @param confidence Confidence of detection
- * @param file Pattern file name
- * @param detected Accepted as true detection, 0 no, 1 yes
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_pattern_detected_send(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[MAVLINK_MSG_ID_PATTERN_DETECTED_LEN];
-	_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;
-	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
-
-// MESSAGE PATTERN_DETECTED UNPACKING
-
-
-/**
- * @brief Get field type from pattern_detected message
- *
- * @return 0: Pattern, 1: Letter
- */
-static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field confidence from pattern_detected message
- *
- * @return Confidence of detection
- */
-static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field file from pattern_detected message
- *
- * @return Pattern file name
- */
-static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, char *file)
-{
-	return _MAV_RETURN_char_array(msg, file, 100,  5);
-}
-
-/**
- * @brief Get field detected from pattern_detected message
- *
- * @return Accepted as true detection, 0 no, 1 yes
- */
-static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  105);
-}
-
-/**
- * @brief Decode a pattern_detected message into a struct
- *
- * @param msg The message to decode
- * @param pattern_detected C-struct to decode the message contents into
- */
-static inline void mavlink_msg_pattern_detected_decode(const mavlink_message_t* msg, mavlink_pattern_detected_t* pattern_detected)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	pattern_detected->confidence = mavlink_msg_pattern_detected_get_confidence(msg);
-	pattern_detected->type = mavlink_msg_pattern_detected_get_type(msg);
-	mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file);
-	pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg);
-#else
-	memcpy(pattern_detected, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
-#endif
-}
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
deleted file mode 100644
index 913a52897..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h
+++ /dev/null
@@ -1,325 +0,0 @@
-// MESSAGE POINT_OF_INTEREST PACKING
-
-#define MAVLINK_MSG_ID_POINT_OF_INTEREST 191
-
-typedef struct __mavlink_point_of_interest_t
-{
- float x; ///< X Position
- float y; ///< Y Position
- float z; ///< Z Position
- uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds
- uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- uint8_t coordinate_system; ///< 0: global, 1:local
- char name[26]; ///< POI name
-} mavlink_point_of_interest_t;
-
-#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 43
-#define MAVLINK_MSG_ID_191_LEN 43
-
-#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC 95
-#define MAVLINK_MSG_ID_191_CRC 95
-
-#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26
-
-#define MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST { \
-	"POINT_OF_INTEREST", \
-	8, \
-	{  { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_point_of_interest_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_point_of_interest_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_point_of_interest_t, z) }, \
-         { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_point_of_interest_t, timeout) }, \
-         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_point_of_interest_t, type) }, \
-         { "color", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_point_of_interest_t, color) }, \
-         { "coordinate_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_point_of_interest_t, coordinate_system) }, \
-         { "name", NULL, MAVLINK_TYPE_CHAR, 26, 17, offsetof(mavlink_point_of_interest_t, name) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a point_of_interest 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 type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- * @param coordinate_system 0: global, 1:local
- * @param timeout 0: no timeout, >1: timeout in seconds
- * @param x X Position
- * @param y Y Position
- * @param z Z Position
- * @param name POI name
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
-#else
-	mavlink_point_of_interest_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
-#endif
-}
-
-/**
- * @brief Pack a point_of_interest 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 type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- * @param coordinate_system 0: global, 1:local
- * @param timeout 0: no timeout, >1: timeout in seconds
- * @param x X Position
- * @param y Y Position
- * @param z Z Position
- * @param name POI name
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
-#else
-	mavlink_point_of_interest_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
-#endif
-}
-
-/**
- * @brief Encode a point_of_interest 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 point_of_interest C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest)
-{
-	return mavlink_msg_point_of_interest_pack(system_id, component_id, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name);
-}
-
-/**
- * @brief Encode a point_of_interest 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 point_of_interest C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_point_of_interest_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest)
-{
-	return mavlink_msg_point_of_interest_pack_chan(system_id, component_id, chan, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name);
-}
-
-/**
- * @brief Send a point_of_interest message
- * @param chan MAVLink channel to send the message
- *
- * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- * @param coordinate_system 0: global, 1:local
- * @param timeout 0: no timeout, >1: timeout in seconds
- * @param x X Position
- * @param y Y Position
- * @param z Z Position
- * @param name POI name
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_point_of_interest_send(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[MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN];
-	_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;
-	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
-
-// MESSAGE POINT_OF_INTEREST UNPACKING
-
-
-/**
- * @brief Get field type from point_of_interest message
- *
- * @return 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- */
-static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Get field color from point_of_interest message
- *
- * @return 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- */
-static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  15);
-}
-
-/**
- * @brief Get field coordinate_system from point_of_interest message
- *
- * @return 0: global, 1:local
- */
-static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field timeout from point_of_interest message
- *
- * @return 0: no timeout, >1: timeout in seconds
- */
-static inline uint16_t mavlink_msg_point_of_interest_get_timeout(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field x from point_of_interest message
- *
- * @return X Position
- */
-static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field y from point_of_interest message
- *
- * @return Y Position
- */
-static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field z from point_of_interest message
- *
- * @return Z Position
- */
-static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field name from point_of_interest message
- *
- * @return POI name
- */
-static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, char *name)
-{
-	return _MAV_RETURN_char_array(msg, name, 26,  17);
-}
-
-/**
- * @brief Decode a point_of_interest message into a struct
- *
- * @param msg The message to decode
- * @param point_of_interest C-struct to decode the message contents into
- */
-static inline void mavlink_msg_point_of_interest_decode(const mavlink_message_t* msg, mavlink_point_of_interest_t* point_of_interest)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	point_of_interest->x = mavlink_msg_point_of_interest_get_x(msg);
-	point_of_interest->y = mavlink_msg_point_of_interest_get_y(msg);
-	point_of_interest->z = mavlink_msg_point_of_interest_get_z(msg);
-	point_of_interest->timeout = mavlink_msg_point_of_interest_get_timeout(msg);
-	point_of_interest->type = mavlink_msg_point_of_interest_get_type(msg);
-	point_of_interest->color = mavlink_msg_point_of_interest_get_color(msg);
-	point_of_interest->coordinate_system = mavlink_msg_point_of_interest_get_coordinate_system(msg);
-	mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name);
-#else
-	memcpy(point_of_interest, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
-#endif
-}
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
deleted file mode 100644
index e24436431..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h
+++ /dev/null
@@ -1,391 +0,0 @@
-// MESSAGE POINT_OF_INTEREST_CONNECTION PACKING
-
-#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 192
-
-typedef struct __mavlink_point_of_interest_connection_t
-{
- float xp1; ///< X1 Position
- float yp1; ///< Y1 Position
- float zp1; ///< Z1 Position
- float xp2; ///< X2 Position
- float yp2; ///< Y2 Position
- float zp2; ///< Z2 Position
- uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds
- uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- uint8_t coordinate_system; ///< 0: global, 1:local
- char name[26]; ///< POI connection name
-} mavlink_point_of_interest_connection_t;
-
-#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 55
-#define MAVLINK_MSG_ID_192_LEN 55
-
-#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC 36
-#define MAVLINK_MSG_ID_192_CRC 36
-
-#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26
-
-#define MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION { \
-	"POINT_OF_INTEREST_CONNECTION", \
-	11, \
-	{  { "xp1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_point_of_interest_connection_t, xp1) }, \
-         { "yp1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_point_of_interest_connection_t, yp1) }, \
-         { "zp1", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_point_of_interest_connection_t, zp1) }, \
-         { "xp2", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_point_of_interest_connection_t, xp2) }, \
-         { "yp2", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_point_of_interest_connection_t, yp2) }, \
-         { "zp2", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_point_of_interest_connection_t, zp2) }, \
-         { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_point_of_interest_connection_t, timeout) }, \
-         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_point_of_interest_connection_t, type) }, \
-         { "color", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_point_of_interest_connection_t, color) }, \
-         { "coordinate_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_point_of_interest_connection_t, coordinate_system) }, \
-         { "name", NULL, MAVLINK_TYPE_CHAR, 26, 29, offsetof(mavlink_point_of_interest_connection_t, name) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a point_of_interest_connection 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 type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- * @param coordinate_system 0: global, 1:local
- * @param timeout 0: no timeout, >1: timeout in seconds
- * @param xp1 X1 Position
- * @param yp1 Y1 Position
- * @param zp1 Z1 Position
- * @param xp2 X2 Position
- * @param yp2 Y2 Position
- * @param zp2 Z2 Position
- * @param name POI connection name
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
-#else
-	mavlink_point_of_interest_connection_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
-#endif
-}
-
-/**
- * @brief Pack a point_of_interest_connection 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 type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- * @param coordinate_system 0: global, 1:local
- * @param timeout 0: no timeout, >1: timeout in seconds
- * @param xp1 X1 Position
- * @param yp1 Y1 Position
- * @param zp1 Z1 Position
- * @param xp2 X2 Position
- * @param yp2 Y2 Position
- * @param zp2 Z2 Position
- * @param name POI connection name
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
-#else
-	mavlink_point_of_interest_connection_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
-#endif
-}
-
-/**
- * @brief Encode a point_of_interest_connection 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 point_of_interest_connection C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection)
-{
-	return mavlink_msg_point_of_interest_connection_pack(system_id, component_id, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name);
-}
-
-/**
- * @brief Encode a point_of_interest_connection 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 point_of_interest_connection C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_point_of_interest_connection_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection)
-{
-	return mavlink_msg_point_of_interest_connection_pack_chan(system_id, component_id, chan, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name);
-}
-
-/**
- * @brief Send a point_of_interest_connection message
- * @param chan MAVLink channel to send the message
- *
- * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- * @param coordinate_system 0: global, 1:local
- * @param timeout 0: no timeout, >1: timeout in seconds
- * @param xp1 X1 Position
- * @param yp1 Y1 Position
- * @param zp1 Z1 Position
- * @param xp2 X2 Position
- * @param yp2 Y2 Position
- * @param zp2 Z2 Position
- * @param name POI connection name
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_point_of_interest_connection_send(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[MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN];
-	_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;
-	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
-
-// MESSAGE POINT_OF_INTEREST_CONNECTION UNPACKING
-
-
-/**
- * @brief Get field type from point_of_interest_connection message
- *
- * @return 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug
- */
-static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  26);
-}
-
-/**
- * @brief Get field color from point_of_interest_connection message
- *
- * @return 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta
- */
-static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  27);
-}
-
-/**
- * @brief Get field coordinate_system from point_of_interest_connection message
- *
- * @return 0: global, 1:local
- */
-static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  28);
-}
-
-/**
- * @brief Get field timeout from point_of_interest_connection message
- *
- * @return 0: no timeout, >1: timeout in seconds
- */
-static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  24);
-}
-
-/**
- * @brief Get field xp1 from point_of_interest_connection message
- *
- * @return X1 Position
- */
-static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field yp1 from point_of_interest_connection message
- *
- * @return Y1 Position
- */
-static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field zp1 from point_of_interest_connection message
- *
- * @return Z1 Position
- */
-static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field xp2 from point_of_interest_connection message
- *
- * @return X2 Position
- */
-static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field yp2 from point_of_interest_connection message
- *
- * @return Y2 Position
- */
-static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field zp2 from point_of_interest_connection message
- *
- * @return Z2 Position
- */
-static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field name from point_of_interest_connection message
- *
- * @return POI connection name
- */
-static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, char *name)
-{
-	return _MAV_RETURN_char_array(msg, name, 26,  29);
-}
-
-/**
- * @brief Decode a point_of_interest_connection message into a struct
- *
- * @param msg The message to decode
- * @param point_of_interest_connection C-struct to decode the message contents into
- */
-static inline void mavlink_msg_point_of_interest_connection_decode(const mavlink_message_t* msg, mavlink_point_of_interest_connection_t* point_of_interest_connection)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	point_of_interest_connection->xp1 = mavlink_msg_point_of_interest_connection_get_xp1(msg);
-	point_of_interest_connection->yp1 = mavlink_msg_point_of_interest_connection_get_yp1(msg);
-	point_of_interest_connection->zp1 = mavlink_msg_point_of_interest_connection_get_zp1(msg);
-	point_of_interest_connection->xp2 = mavlink_msg_point_of_interest_connection_get_xp2(msg);
-	point_of_interest_connection->yp2 = mavlink_msg_point_of_interest_connection_get_yp2(msg);
-	point_of_interest_connection->zp2 = mavlink_msg_point_of_interest_connection_get_zp2(msg);
-	point_of_interest_connection->timeout = mavlink_msg_point_of_interest_connection_get_timeout(msg);
-	point_of_interest_connection->type = mavlink_msg_point_of_interest_connection_get_type(msg);
-	point_of_interest_connection->color = mavlink_msg_point_of_interest_connection_get_color(msg);
-	point_of_interest_connection->coordinate_system = mavlink_msg_point_of_interest_connection_get_coordinate_system(msg);
-	mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name);
-#else
-	memcpy(point_of_interest_connection, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
-#endif
-}
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
deleted file mode 100644
index 6f4ca510a..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h
+++ /dev/null
@@ -1,265 +0,0 @@
-// MESSAGE POSITION_CONTROL_SETPOINT PACKING
-
-#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 170
-
-typedef struct __mavlink_position_control_setpoint_t
-{
- float x; ///< x position
- float y; ///< y position
- float z; ///< z position
- float yaw; ///< yaw orientation in radians, 0 = NORTH
- uint16_t id; ///< ID of waypoint, 0 for plain position
-} mavlink_position_control_setpoint_t;
-
-#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN 18
-#define MAVLINK_MSG_ID_170_LEN 18
-
-#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC 28
-#define MAVLINK_MSG_ID_170_CRC 28
-
-
-
-#define MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT { \
-	"POSITION_CONTROL_SETPOINT", \
-	5, \
-	{  { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_control_setpoint_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_control_setpoint_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_control_setpoint_t, z) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_control_setpoint_t, yaw) }, \
-         { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_position_control_setpoint_t, id) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a position_control_setpoint 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 id ID of waypoint, 0 for plain position
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw yaw orientation in radians, 0 = NORTH
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t id, float x, float y, float z, float yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
-#else
-	mavlink_position_control_setpoint_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.yaw = yaw;
-	packet.id = id;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a position_control_setpoint 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 id ID of waypoint, 0 for plain position
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw yaw orientation in radians, 0 = NORTH
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t id,float x,float y,float z,float yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
-#else
-	mavlink_position_control_setpoint_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.yaw = yaw;
-	packet.id = id;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a position_control_setpoint 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 position_control_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint)
-{
-	return mavlink_msg_position_control_setpoint_pack(system_id, component_id, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw);
-}
-
-/**
- * @brief Encode a position_control_setpoint 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 position_control_setpoint C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_position_control_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint)
-{
-	return mavlink_msg_position_control_setpoint_pack_chan(system_id, component_id, chan, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw);
-}
-
-/**
- * @brief Send a position_control_setpoint message
- * @param chan MAVLink channel to send the message
- *
- * @param id ID of waypoint, 0 for plain position
- * @param x x position
- * @param y y position
- * @param z z position
- * @param yaw yaw orientation in radians, 0 = NORTH
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_position_control_setpoint_send(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[MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN];
-	_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;
-	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
-
-// MESSAGE POSITION_CONTROL_SETPOINT UNPACKING
-
-
-/**
- * @brief Get field id from position_control_setpoint message
- *
- * @return ID of waypoint, 0 for plain position
- */
-static inline uint16_t mavlink_msg_position_control_setpoint_get_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field x from position_control_setpoint message
- *
- * @return x position
- */
-static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field y from position_control_setpoint message
- *
- * @return y position
- */
-static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field z from position_control_setpoint message
- *
- * @return z position
- */
-static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw from position_control_setpoint message
- *
- * @return yaw orientation in radians, 0 = NORTH
- */
-static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Decode a position_control_setpoint message into a struct
- *
- * @param msg The message to decode
- * @param position_control_setpoint C-struct to decode the message contents into
- */
-static inline void mavlink_msg_position_control_setpoint_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_t* position_control_setpoint)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	position_control_setpoint->x = mavlink_msg_position_control_setpoint_get_x(msg);
-	position_control_setpoint->y = mavlink_msg_position_control_setpoint_get_y(msg);
-	position_control_setpoint->z = mavlink_msg_position_control_setpoint_get_z(msg);
-	position_control_setpoint->yaw = mavlink_msg_position_control_setpoint_get_yaw(msg);
-	position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg);
-#else
-	memcpy(position_control_setpoint, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
-#endif
-}
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
deleted file mode 100644
index 08cedbb4b..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h
+++ /dev/null
@@ -1,309 +0,0 @@
-// MESSAGE RAW_AUX PACKING
-
-#define MAVLINK_MSG_ID_RAW_AUX 172
-
-typedef struct __mavlink_raw_aux_t
-{
- int32_t baro; ///< Barometric pressure (hecto Pascal)
- uint16_t adc1; ///< ADC1 (J405 ADC3, LPC2148 AD0.6)
- uint16_t adc2; ///< ADC2 (J405 ADC5, LPC2148 AD0.2)
- uint16_t adc3; ///< ADC3 (J405 ADC6, LPC2148 AD0.1)
- uint16_t adc4; ///< ADC4 (J405 ADC7, LPC2148 AD1.3)
- uint16_t vbat; ///< Battery voltage
- int16_t temp; ///< Temperature (degrees celcius)
-} mavlink_raw_aux_t;
-
-#define MAVLINK_MSG_ID_RAW_AUX_LEN 16
-#define MAVLINK_MSG_ID_172_LEN 16
-
-#define MAVLINK_MSG_ID_RAW_AUX_CRC 182
-#define MAVLINK_MSG_ID_172_CRC 182
-
-
-
-#define MAVLINK_MESSAGE_INFO_RAW_AUX { \
-	"RAW_AUX", \
-	7, \
-	{  { "baro", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_raw_aux_t, baro) }, \
-         { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_raw_aux_t, adc1) }, \
-         { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_raw_aux_t, adc2) }, \
-         { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_raw_aux_t, adc3) }, \
-         { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_raw_aux_t, adc4) }, \
-         { "vbat", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_raw_aux_t, vbat) }, \
-         { "temp", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_aux_t, temp) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a raw_aux 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 adc1 ADC1 (J405 ADC3, LPC2148 AD0.6)
- * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2)
- * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1)
- * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3)
- * @param vbat Battery voltage
- * @param temp Temperature (degrees celcius)
- * @param baro Barometric pressure (hecto Pascal)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_RAW_AUX_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_AUX_LEN);
-#else
-	mavlink_raw_aux_t packet;
-	packet.baro = baro;
-	packet.adc1 = adc1;
-	packet.adc2 = adc2;
-	packet.adc3 = adc3;
-	packet.adc4 = adc4;
-	packet.vbat = vbat;
-	packet.temp = temp;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_AUX_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RAW_AUX;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN);
-#endif
-}
-
-/**
- * @brief Pack a raw_aux 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 adc1 ADC1 (J405 ADC3, LPC2148 AD0.6)
- * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2)
- * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1)
- * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3)
- * @param vbat Battery voltage
- * @param temp Temperature (degrees celcius)
- * @param baro Barometric pressure (hecto Pascal)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_RAW_AUX_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_AUX_LEN);
-#else
-	mavlink_raw_aux_t packet;
-	packet.baro = baro;
-	packet.adc1 = adc1;
-	packet.adc2 = adc2;
-	packet.adc3 = adc3;
-	packet.adc4 = adc4;
-	packet.vbat = vbat;
-	packet.temp = temp;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_AUX_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RAW_AUX;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN);
-#endif
-}
-
-/**
- * @brief Encode a raw_aux 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 raw_aux C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux)
-{
-	return mavlink_msg_raw_aux_pack(system_id, component_id, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro);
-}
-
-/**
- * @brief Encode a raw_aux 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 raw_aux C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_raw_aux_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux)
-{
-	return mavlink_msg_raw_aux_pack_chan(system_id, component_id, chan, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro);
-}
-
-/**
- * @brief Send a raw_aux message
- * @param chan MAVLink channel to send the message
- *
- * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6)
- * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2)
- * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1)
- * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3)
- * @param vbat Battery voltage
- * @param temp Temperature (degrees celcius)
- * @param baro Barometric pressure (hecto Pascal)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_raw_aux_send(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[MAVLINK_MSG_ID_RAW_AUX_LEN];
-	_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;
-	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
-
-// MESSAGE RAW_AUX UNPACKING
-
-
-/**
- * @brief Get field adc1 from raw_aux message
- *
- * @return ADC1 (J405 ADC3, LPC2148 AD0.6)
- */
-static inline uint16_t mavlink_msg_raw_aux_get_adc1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field adc2 from raw_aux message
- *
- * @return ADC2 (J405 ADC5, LPC2148 AD0.2)
- */
-static inline uint16_t mavlink_msg_raw_aux_get_adc2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field adc3 from raw_aux message
- *
- * @return ADC3 (J405 ADC6, LPC2148 AD0.1)
- */
-static inline uint16_t mavlink_msg_raw_aux_get_adc3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field adc4 from raw_aux message
- *
- * @return ADC4 (J405 ADC7, LPC2148 AD1.3)
- */
-static inline uint16_t mavlink_msg_raw_aux_get_adc4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Get field vbat from raw_aux message
- *
- * @return Battery voltage
- */
-static inline uint16_t mavlink_msg_raw_aux_get_vbat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field temp from raw_aux message
- *
- * @return Temperature (degrees celcius)
- */
-static inline int16_t mavlink_msg_raw_aux_get_temp(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  14);
-}
-
-/**
- * @brief Get field baro from raw_aux message
- *
- * @return Barometric pressure (hecto Pascal)
- */
-static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Decode a raw_aux message into a struct
- *
- * @param msg The message to decode
- * @param raw_aux C-struct to decode the message contents into
- */
-static inline void mavlink_msg_raw_aux_decode(const mavlink_message_t* msg, mavlink_raw_aux_t* raw_aux)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	raw_aux->baro = mavlink_msg_raw_aux_get_baro(msg);
-	raw_aux->adc1 = mavlink_msg_raw_aux_get_adc1(msg);
-	raw_aux->adc2 = mavlink_msg_raw_aux_get_adc2(msg);
-	raw_aux->adc3 = mavlink_msg_raw_aux_get_adc3(msg);
-	raw_aux->adc4 = mavlink_msg_raw_aux_get_adc4(msg);
-	raw_aux->vbat = mavlink_msg_raw_aux_get_vbat(msg);
-	raw_aux->temp = mavlink_msg_raw_aux_get_temp(msg);
-#else
-	memcpy(raw_aux, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RAW_AUX_LEN);
-#endif
-}
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
deleted file mode 100644
index b26d748c2..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE SET_CAM_SHUTTER PACKING
-
-#define MAVLINK_MSG_ID_SET_CAM_SHUTTER 151
-
-typedef struct __mavlink_set_cam_shutter_t
-{
- float gain; ///< Camera gain
- uint16_t interval; ///< Shutter interval, in microseconds
- uint16_t exposure; ///< Exposure time, in microseconds
- uint8_t cam_no; ///< Camera id
- uint8_t cam_mode; ///< Camera mode: 0 = auto, 1 = manual
- uint8_t trigger_pin; ///< Trigger pin, 0-3 for PtGrey FireFly
-} mavlink_set_cam_shutter_t;
-
-#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN 11
-#define MAVLINK_MSG_ID_151_LEN 11
-
-#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC 108
-#define MAVLINK_MSG_ID_151_CRC 108
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER { \
-	"SET_CAM_SHUTTER", \
-	6, \
-	{  { "gain", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_cam_shutter_t, gain) }, \
-         { "interval", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_set_cam_shutter_t, interval) }, \
-         { "exposure", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_set_cam_shutter_t, exposure) }, \
-         { "cam_no", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_set_cam_shutter_t, cam_no) }, \
-         { "cam_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_set_cam_shutter_t, cam_mode) }, \
-         { "trigger_pin", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_set_cam_shutter_t, trigger_pin) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_cam_shutter 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 cam_no Camera id
- * @param cam_mode Camera mode: 0 = auto, 1 = manual
- * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly
- * @param interval Shutter interval, in microseconds
- * @param exposure Exposure time, in microseconds
- * @param gain Camera gain
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
-#else
-	mavlink_set_cam_shutter_t packet;
-	packet.gain = gain;
-	packet.interval = interval;
-	packet.exposure = exposure;
-	packet.cam_no = cam_no;
-	packet.cam_mode = cam_mode;
-	packet.trigger_pin = trigger_pin;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_cam_shutter 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 cam_no Camera id
- * @param cam_mode Camera mode: 0 = auto, 1 = manual
- * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly
- * @param interval Shutter interval, in microseconds
- * @param exposure Exposure time, in microseconds
- * @param gain Camera gain
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
-#else
-	mavlink_set_cam_shutter_t packet;
-	packet.gain = gain;
-	packet.interval = interval;
-	packet.exposure = exposure;
-	packet.cam_no = cam_no;
-	packet.cam_mode = cam_mode;
-	packet.trigger_pin = trigger_pin;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_cam_shutter 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 set_cam_shutter C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter)
-{
-	return mavlink_msg_set_cam_shutter_pack(system_id, component_id, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain);
-}
-
-/**
- * @brief Encode a set_cam_shutter 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 set_cam_shutter C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_cam_shutter_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter)
-{
-	return mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, chan, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain);
-}
-
-/**
- * @brief Send a set_cam_shutter message
- * @param chan MAVLink channel to send the message
- *
- * @param cam_no Camera id
- * @param cam_mode Camera mode: 0 = auto, 1 = manual
- * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly
- * @param interval Shutter interval, in microseconds
- * @param exposure Exposure time, in microseconds
- * @param gain Camera gain
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_cam_shutter_send(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[MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN];
-	_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;
-	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
-
-// MESSAGE SET_CAM_SHUTTER UNPACKING
-
-
-/**
- * @brief Get field cam_no from set_cam_shutter message
- *
- * @return Camera id
- */
-static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field cam_mode from set_cam_shutter message
- *
- * @return Camera mode: 0 = auto, 1 = manual
- */
-static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Get field trigger_pin from set_cam_shutter message
- *
- * @return Trigger pin, 0-3 for PtGrey FireFly
- */
-static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field interval from set_cam_shutter message
- *
- * @return Shutter interval, in microseconds
- */
-static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field exposure from set_cam_shutter message
- *
- * @return Exposure time, in microseconds
- */
-static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field gain from set_cam_shutter message
- *
- * @return Camera gain
- */
-static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Decode a set_cam_shutter message into a struct
- *
- * @param msg The message to decode
- * @param set_cam_shutter C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_cam_shutter_decode(const mavlink_message_t* msg, mavlink_set_cam_shutter_t* set_cam_shutter)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_cam_shutter->gain = mavlink_msg_set_cam_shutter_get_gain(msg);
-	set_cam_shutter->interval = mavlink_msg_set_cam_shutter_get_interval(msg);
-	set_cam_shutter->exposure = mavlink_msg_set_cam_shutter_get_exposure(msg);
-	set_cam_shutter->cam_no = mavlink_msg_set_cam_shutter_get_cam_no(msg);
-	set_cam_shutter->cam_mode = mavlink_msg_set_cam_shutter_get_cam_mode(msg);
-	set_cam_shutter->trigger_pin = mavlink_msg_set_cam_shutter_get_trigger_pin(msg);
-#else
-	memcpy(set_cam_shutter, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
-#endif
-}
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
deleted file mode 100644
index f1f9e698f..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE SET_POSITION_CONTROL_OFFSET PACKING
-
-#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET 160
-
-typedef struct __mavlink_set_position_control_offset_t
-{
- float x; ///< x position offset
- float y; ///< y position offset
- float z; ///< z position offset
- float yaw; ///< yaw orientation offset in radians, 0 = NORTH
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_set_position_control_offset_t;
-
-#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN 18
-#define MAVLINK_MSG_ID_160_LEN 18
-
-#define MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC 22
-#define MAVLINK_MSG_ID_160_CRC 22
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET { \
-	"SET_POSITION_CONTROL_OFFSET", \
-	6, \
-	{  { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_position_control_offset_t, x) }, \
-         { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_position_control_offset_t, y) }, \
-         { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_position_control_offset_t, z) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_position_control_offset_t, yaw) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_position_control_offset_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_position_control_offset_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_position_control_offset 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 target_system System ID
- * @param target_component Component ID
- * @param x x position offset
- * @param y y position offset
- * @param z z position offset
- * @param yaw yaw orientation offset in radians, 0 = NORTH
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
-#else
-	mavlink_set_position_control_offset_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.yaw = yaw;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_position_control_offset 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 target_system System ID
- * @param target_component Component ID
- * @param x x position offset
- * @param y y position offset
- * @param z z position offset
- * @param yaw yaw orientation offset in radians, 0 = NORTH
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_position_control_offset_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
-#else
-	mavlink_set_position_control_offset_t packet;
-	packet.x = x;
-	packet.y = y;
-	packet.z = z;
-	packet.yaw = yaw;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_position_control_offset 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 set_position_control_offset C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_position_control_offset_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset)
-{
-	return mavlink_msg_set_position_control_offset_pack(system_id, component_id, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw);
-}
-
-/**
- * @brief Encode a set_position_control_offset 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 set_position_control_offset C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_position_control_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset)
-{
-	return mavlink_msg_set_position_control_offset_pack_chan(system_id, component_id, chan, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw);
-}
-
-/**
- * @brief Send a set_position_control_offset message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param x x position offset
- * @param y y position offset
- * @param z z position offset
- * @param yaw yaw orientation offset in radians, 0 = NORTH
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_position_control_offset_send(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[MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN];
-	_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;
-	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
-
-// MESSAGE SET_POSITION_CONTROL_OFFSET UNPACKING
-
-
-/**
- * @brief Get field target_system from set_position_control_offset message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_set_position_control_offset_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  16);
-}
-
-/**
- * @brief Get field target_component from set_position_control_offset message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_set_position_control_offset_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  17);
-}
-
-/**
- * @brief Get field x from set_position_control_offset message
- *
- * @return x position offset
- */
-static inline float mavlink_msg_set_position_control_offset_get_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field y from set_position_control_offset message
- *
- * @return y position offset
- */
-static inline float mavlink_msg_set_position_control_offset_get_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field z from set_position_control_offset message
- *
- * @return z position offset
- */
-static inline float mavlink_msg_set_position_control_offset_get_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw from set_position_control_offset message
- *
- * @return yaw orientation offset in radians, 0 = NORTH
- */
-static inline float mavlink_msg_set_position_control_offset_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Decode a set_position_control_offset message into a struct
- *
- * @param msg The message to decode
- * @param set_position_control_offset C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_position_control_offset_decode(const mavlink_message_t* msg, mavlink_set_position_control_offset_t* set_position_control_offset)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_position_control_offset->x = mavlink_msg_set_position_control_offset_get_x(msg);
-	set_position_control_offset->y = mavlink_msg_set_position_control_offset_get_y(msg);
-	set_position_control_offset->z = mavlink_msg_set_position_control_offset_get_z(msg);
-	set_position_control_offset->yaw = mavlink_msg_set_position_control_offset_get_yaw(msg);
-	set_position_control_offset->target_system = mavlink_msg_set_position_control_offset_get_target_system(msg);
-	set_position_control_offset->target_component = mavlink_msg_set_position_control_offset_get_target_component(msg);
-#else
-	memcpy(set_position_control_offset, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
-#endif
-}
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
deleted file mode 100644
index 9458087da..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// MESSAGE WATCHDOG_COMMAND PACKING
-
-#define MAVLINK_MSG_ID_WATCHDOG_COMMAND 183
-
-typedef struct __mavlink_watchdog_command_t
-{
- uint16_t watchdog_id; ///< Watchdog ID
- uint16_t process_id; ///< Process ID
- uint8_t target_system_id; ///< Target system ID
- uint8_t command_id; ///< Command ID
-} mavlink_watchdog_command_t;
-
-#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN 6
-#define MAVLINK_MSG_ID_183_LEN 6
-
-#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC 162
-#define MAVLINK_MSG_ID_183_CRC 162
-
-
-
-#define MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND { \
-	"WATCHDOG_COMMAND", \
-	4, \
-	{  { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_command_t, watchdog_id) }, \
-         { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_command_t, process_id) }, \
-         { "target_system_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_watchdog_command_t, target_system_id) }, \
-         { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_watchdog_command_t, command_id) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a watchdog_command 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 target_system_id Target system ID
- * @param watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param command_id Command ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
-#else
-	mavlink_watchdog_command_t packet;
-	packet.watchdog_id = watchdog_id;
-	packet.process_id = process_id;
-	packet.target_system_id = target_system_id;
-	packet.command_id = command_id;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
-#endif
-}
-
-/**
- * @brief Pack a watchdog_command 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 target_system_id Target system ID
- * @param watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param command_id Command ID
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
-#else
-	mavlink_watchdog_command_t packet;
-	packet.watchdog_id = watchdog_id;
-	packet.process_id = process_id;
-	packet.target_system_id = target_system_id;
-	packet.command_id = command_id;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
-#endif
-}
-
-/**
- * @brief Encode a watchdog_command 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 watchdog_command C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command)
-{
-	return mavlink_msg_watchdog_command_pack(system_id, component_id, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id);
-}
-
-/**
- * @brief Encode a watchdog_command 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 watchdog_command C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_watchdog_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command)
-{
-	return mavlink_msg_watchdog_command_pack_chan(system_id, component_id, chan, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id);
-}
-
-/**
- * @brief Send a watchdog_command message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system_id Target system ID
- * @param watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param command_id Command ID
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_watchdog_command_send(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[MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN];
-	_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;
-	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
-
-// MESSAGE WATCHDOG_COMMAND UNPACKING
-
-
-/**
- * @brief Get field target_system_id from watchdog_command message
- *
- * @return Target system ID
- */
-static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field watchdog_id from watchdog_command message
- *
- * @return Watchdog ID
- */
-static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field process_id from watchdog_command message
- *
- * @return Process ID
- */
-static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Get field command_id from watchdog_command message
- *
- * @return Command ID
- */
-static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Decode a watchdog_command message into a struct
- *
- * @param msg The message to decode
- * @param watchdog_command C-struct to decode the message contents into
- */
-static inline void mavlink_msg_watchdog_command_decode(const mavlink_message_t* msg, mavlink_watchdog_command_t* watchdog_command)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	watchdog_command->watchdog_id = mavlink_msg_watchdog_command_get_watchdog_id(msg);
-	watchdog_command->process_id = mavlink_msg_watchdog_command_get_process_id(msg);
-	watchdog_command->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg);
-	watchdog_command->command_id = mavlink_msg_watchdog_command_get_command_id(msg);
-#else
-	memcpy(watchdog_command, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
-#endif
-}
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
deleted file mode 100644
index 3f4295ee5..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h
+++ /dev/null
@@ -1,199 +0,0 @@
-// MESSAGE WATCHDOG_HEARTBEAT PACKING
-
-#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 180
-
-typedef struct __mavlink_watchdog_heartbeat_t
-{
- uint16_t watchdog_id; ///< Watchdog ID
- uint16_t process_count; ///< Number of processes
-} mavlink_watchdog_heartbeat_t;
-
-#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4
-#define MAVLINK_MSG_ID_180_LEN 4
-
-#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC 153
-#define MAVLINK_MSG_ID_180_CRC 153
-
-
-
-#define MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT { \
-	"WATCHDOG_HEARTBEAT", \
-	2, \
-	{  { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_heartbeat_t, watchdog_id) }, \
-         { "process_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_heartbeat_t, process_count) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a watchdog_heartbeat 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 watchdog_id Watchdog ID
- * @param process_count Number of processes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t watchdog_id, uint16_t process_count)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN];
-	_mav_put_uint16_t(buf, 0, watchdog_id);
-	_mav_put_uint16_t(buf, 2, process_count);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
-#else
-	mavlink_watchdog_heartbeat_t packet;
-	packet.watchdog_id = watchdog_id;
-	packet.process_count = process_count;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a watchdog_heartbeat 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 watchdog_id Watchdog ID
- * @param process_count Number of processes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t watchdog_id,uint16_t process_count)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN];
-	_mav_put_uint16_t(buf, 0, watchdog_id);
-	_mav_put_uint16_t(buf, 2, process_count);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
-#else
-	mavlink_watchdog_heartbeat_t packet;
-	packet.watchdog_id = watchdog_id;
-	packet.process_count = process_count;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a watchdog_heartbeat 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 watchdog_heartbeat C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat)
-{
-	return mavlink_msg_watchdog_heartbeat_pack(system_id, component_id, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count);
-}
-
-/**
- * @brief Encode a watchdog_heartbeat 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 watchdog_heartbeat C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_watchdog_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat)
-{
-	return mavlink_msg_watchdog_heartbeat_pack_chan(system_id, component_id, chan, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count);
-}
-
-/**
- * @brief Send a watchdog_heartbeat message
- * @param chan MAVLink channel to send the message
- *
- * @param watchdog_id Watchdog ID
- * @param process_count Number of processes
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN];
-	_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;
-	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
-
-// MESSAGE WATCHDOG_HEARTBEAT UNPACKING
-
-
-/**
- * @brief Get field watchdog_id from watchdog_heartbeat message
- *
- * @return Watchdog ID
- */
-static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field process_count from watchdog_heartbeat message
- *
- * @return Number of processes
- */
-static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a watchdog_heartbeat message into a struct
- *
- * @param msg The message to decode
- * @param watchdog_heartbeat C-struct to decode the message contents into
- */
-static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t* msg, mavlink_watchdog_heartbeat_t* watchdog_heartbeat)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg);
-	watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg);
-#else
-	memcpy(watchdog_heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
-#endif
-}
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
deleted file mode 100644
index 55853cdde..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h
+++ /dev/null
@@ -1,260 +0,0 @@
-// MESSAGE WATCHDOG_PROCESS_INFO PACKING
-
-#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 181
-
-typedef struct __mavlink_watchdog_process_info_t
-{
- int32_t timeout; ///< Timeout (seconds)
- uint16_t watchdog_id; ///< Watchdog ID
- uint16_t process_id; ///< Process ID
- char name[100]; ///< Process name
- char arguments[147]; ///< Process arguments
-} mavlink_watchdog_process_info_t;
-
-#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN 255
-#define MAVLINK_MSG_ID_181_LEN 255
-
-#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC 16
-#define MAVLINK_MSG_ID_181_CRC 16
-
-#define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100
-#define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_ARGUMENTS_LEN 147
-
-#define MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO { \
-	"WATCHDOG_PROCESS_INFO", \
-	5, \
-	{  { "timeout", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_watchdog_process_info_t, timeout) }, \
-         { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_watchdog_process_info_t, watchdog_id) }, \
-         { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_watchdog_process_info_t, process_id) }, \
-         { "name", NULL, MAVLINK_TYPE_CHAR, 100, 8, offsetof(mavlink_watchdog_process_info_t, name) }, \
-         { "arguments", NULL, MAVLINK_TYPE_CHAR, 147, 108, offsetof(mavlink_watchdog_process_info_t, arguments) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a watchdog_process_info 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 watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param name Process name
- * @param arguments Process arguments
- * @param timeout Timeout (seconds)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
-#else
-	mavlink_watchdog_process_info_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
-#endif
-}
-
-/**
- * @brief Pack a watchdog_process_info 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 watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param name Process name
- * @param arguments Process arguments
- * @param timeout Timeout (seconds)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN];
-	_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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
-#else
-	mavlink_watchdog_process_info_t packet;
-	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);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
-#endif
-}
-
-/**
- * @brief Encode a watchdog_process_info 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 watchdog_process_info C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info)
-{
-	return mavlink_msg_watchdog_process_info_pack(system_id, component_id, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout);
-}
-
-/**
- * @brief Encode a watchdog_process_info 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 watchdog_process_info C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_watchdog_process_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info)
-{
-	return mavlink_msg_watchdog_process_info_pack_chan(system_id, component_id, chan, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout);
-}
-
-/**
- * @brief Send a watchdog_process_info message
- * @param chan MAVLink channel to send the message
- *
- * @param watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param name Process name
- * @param arguments Process arguments
- * @param timeout Timeout (seconds)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_watchdog_process_info_send(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[MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN];
-	_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;
-	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
-
-// MESSAGE WATCHDOG_PROCESS_INFO UNPACKING
-
-
-/**
- * @brief Get field watchdog_id from watchdog_process_info message
- *
- * @return Watchdog ID
- */
-static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field process_id from watchdog_process_info message
- *
- * @return Process ID
- */
-static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field name from watchdog_process_info message
- *
- * @return Process name
- */
-static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, char *name)
-{
-	return _MAV_RETURN_char_array(msg, name, 100,  8);
-}
-
-/**
- * @brief Get field arguments from watchdog_process_info message
- *
- * @return Process arguments
- */
-static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, char *arguments)
-{
-	return _MAV_RETURN_char_array(msg, arguments, 147,  108);
-}
-
-/**
- * @brief Get field timeout from watchdog_process_info message
- *
- * @return Timeout (seconds)
- */
-static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Decode a watchdog_process_info message into a struct
- *
- * @param msg The message to decode
- * @param watchdog_process_info C-struct to decode the message contents into
- */
-static inline void mavlink_msg_watchdog_process_info_decode(const mavlink_message_t* msg, mavlink_watchdog_process_info_t* watchdog_process_info)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	watchdog_process_info->timeout = mavlink_msg_watchdog_process_info_get_timeout(msg);
-	watchdog_process_info->watchdog_id = mavlink_msg_watchdog_process_info_get_watchdog_id(msg);
-	watchdog_process_info->process_id = mavlink_msg_watchdog_process_info_get_process_id(msg);
-	mavlink_msg_watchdog_process_info_get_name(msg, watchdog_process_info->name);
-	mavlink_msg_watchdog_process_info_get_arguments(msg, watchdog_process_info->arguments);
-#else
-	memcpy(watchdog_process_info, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
-#endif
-}
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
deleted file mode 100644
index a0410d803..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h
+++ /dev/null
@@ -1,287 +0,0 @@
-// MESSAGE WATCHDOG_PROCESS_STATUS PACKING
-
-#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 182
-
-typedef struct __mavlink_watchdog_process_status_t
-{
- int32_t pid; ///< PID
- uint16_t watchdog_id; ///< Watchdog ID
- uint16_t process_id; ///< Process ID
- uint16_t crashes; ///< Number of crashes
- uint8_t state; ///< Is running / finished / suspended / crashed
- uint8_t muted; ///< Is muted
-} mavlink_watchdog_process_status_t;
-
-#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN 12
-#define MAVLINK_MSG_ID_182_LEN 12
-
-#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC 29
-#define MAVLINK_MSG_ID_182_CRC 29
-
-
-
-#define MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS { \
-	"WATCHDOG_PROCESS_STATUS", \
-	6, \
-	{  { "pid", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_watchdog_process_status_t, pid) }, \
-         { "watchdog_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_watchdog_process_status_t, watchdog_id) }, \
-         { "process_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_watchdog_process_status_t, process_id) }, \
-         { "crashes", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_watchdog_process_status_t, crashes) }, \
-         { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_watchdog_process_status_t, state) }, \
-         { "muted", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_watchdog_process_status_t, muted) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a watchdog_process_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 watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param state Is running / finished / suspended / crashed
- * @param muted Is muted
- * @param pid PID
- * @param crashes Number of crashes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       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[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
-#else
-	mavlink_watchdog_process_status_t packet;
-	packet.pid = pid;
-	packet.watchdog_id = watchdog_id;
-	packet.process_id = process_id;
-	packet.crashes = crashes;
-	packet.state = state;
-	packet.muted = muted;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a watchdog_process_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 watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param state Is running / finished / suspended / crashed
- * @param muted Is muted
- * @param pid PID
- * @param crashes Number of crashes
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           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[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
-#else
-	mavlink_watchdog_process_status_t packet;
-	packet.pid = pid;
-	packet.watchdog_id = watchdog_id;
-	packet.process_id = process_id;
-	packet.crashes = crashes;
-	packet.state = state;
-	packet.muted = muted;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a watchdog_process_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 watchdog_process_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status)
-{
-	return mavlink_msg_watchdog_process_status_pack(system_id, component_id, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes);
-}
-
-/**
- * @brief Encode a watchdog_process_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 watchdog_process_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_watchdog_process_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status)
-{
-	return mavlink_msg_watchdog_process_status_pack_chan(system_id, component_id, chan, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes);
-}
-
-/**
- * @brief Send a watchdog_process_status message
- * @param chan MAVLink channel to send the message
- *
- * @param watchdog_id Watchdog ID
- * @param process_id Process ID
- * @param state Is running / finished / suspended / crashed
- * @param muted Is muted
- * @param pid PID
- * @param crashes Number of crashes
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_watchdog_process_status_send(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[MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN];
-	_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;
-	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
-
-// MESSAGE WATCHDOG_PROCESS_STATUS UNPACKING
-
-
-/**
- * @brief Get field watchdog_id from watchdog_process_status message
- *
- * @return Watchdog ID
- */
-static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field process_id from watchdog_process_status message
- *
- * @return Process ID
- */
-static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field state from watchdog_process_status message
- *
- * @return Is running / finished / suspended / crashed
- */
-static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field muted from watchdog_process_status message
- *
- * @return Is muted
- */
-static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field pid from watchdog_process_status message
- *
- * @return PID
- */
-static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field crashes from watchdog_process_status message
- *
- * @return Number of crashes
- */
-static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Decode a watchdog_process_status message into a struct
- *
- * @param msg The message to decode
- * @param watchdog_process_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_watchdog_process_status_decode(const mavlink_message_t* msg, mavlink_watchdog_process_status_t* watchdog_process_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	watchdog_process_status->pid = mavlink_msg_watchdog_process_status_get_pid(msg);
-	watchdog_process_status->watchdog_id = mavlink_msg_watchdog_process_status_get_watchdog_id(msg);
-	watchdog_process_status->process_id = mavlink_msg_watchdog_process_status_get_process_id(msg);
-	watchdog_process_status->crashes = mavlink_msg_watchdog_process_status_get_crashes(msg);
-	watchdog_process_status->state = mavlink_msg_watchdog_process_status_get_state(msg);
-	watchdog_process_status->muted = mavlink_msg_watchdog_process_status_get_muted(msg);
-#else
-	memcpy(watchdog_process_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
deleted file mode 100644
index 428619eed..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol generated from pixhawk.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef PIXHAWK_H
-#define PIXHAWK_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// 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}
-#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}
-#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}}}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_PIXHAWK
-
-// ENUM DEFINITIONS
-
-
-/** @brief Content Types for data transmission handshake */
-#ifndef HAVE_ENUM_DATA_TYPES
-#define HAVE_ENUM_DATA_TYPES
-enum DATA_TYPES
-{
-	DATA_TYPE_JPEG_IMAGE=1, /*  | */
-	DATA_TYPE_RAW_IMAGE=2, /*  | */
-	DATA_TYPE_KINECT=3, /*  | */
-	DATA_TYPES_ENUM_END=4, /*  | */
-};
-#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_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, /*  | */
-	MAV_CMD_DO_START_SEARCH=10001, /* Starts a search |1 to arm, 0 to disarm|  */
-	MAV_CMD_DO_FINISH_SEARCH=10002, /* Starts a search |1 to arm, 0 to disarm|  */
-	MAV_CMD_NAV_SWEEP=10003, /* Starts a search |1 to arm, 0 to disarm|  */
-};
-#endif
-
-#include "../common/common.h"
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_set_cam_shutter.h"
-#include "./mavlink_msg_image_triggered.h"
-#include "./mavlink_msg_image_trigger_control.h"
-#include "./mavlink_msg_image_available.h"
-#include "./mavlink_msg_set_position_control_offset.h"
-#include "./mavlink_msg_position_control_setpoint.h"
-#include "./mavlink_msg_marker.h"
-#include "./mavlink_msg_raw_aux.h"
-#include "./mavlink_msg_watchdog_heartbeat.h"
-#include "./mavlink_msg_watchdog_process_info.h"
-#include "./mavlink_msg_watchdog_process_status.h"
-#include "./mavlink_msg_watchdog_command.h"
-#include "./mavlink_msg_pattern_detected.h"
-#include "./mavlink_msg_point_of_interest.h"
-#include "./mavlink_msg_point_of_interest_connection.h"
-#include "./mavlink_msg_brief_feature.h"
-#include "./mavlink_msg_attitude_control.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // PIXHAWK_H
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h
deleted file mode 100644
index 7556606e9..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h
+++ /dev/null
@@ -1,3663 +0,0 @@
-// Generated by the protocol buffer compiler.  DO NOT EDIT!
-// source: pixhawk.proto
-
-#ifndef PROTOBUF_pixhawk_2eproto__INCLUDED
-#define PROTOBUF_pixhawk_2eproto__INCLUDED
-
-#include <string>
-
-#include <google/protobuf/stubs/common.h>
-
-#if GOOGLE_PROTOBUF_VERSION < 2004000
-#error This file was generated by a newer version of protoc which is
-#error incompatible with your Protocol Buffer headers.  Please update
-#error your headers.
-#endif
-#if 2004001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
-#error This file was generated by an older version of protoc which is
-#error incompatible with your Protocol Buffer headers.  Please
-#error regenerate this file with a newer version of protoc.
-#endif
-
-#include <google/protobuf/generated_message_util.h>
-#include <google/protobuf/repeated_field.h>
-#include <google/protobuf/extension_set.h>
-#include <google/protobuf/generated_message_reflection.h>
-// @@protoc_insertion_point(includes)
-
-namespace px {
-
-// Internal implementation detail -- do not call these.
-void  protobuf_AddDesc_pixhawk_2eproto();
-void protobuf_AssignDesc_pixhawk_2eproto();
-void protobuf_ShutdownFile_pixhawk_2eproto();
-
-class HeaderInfo;
-class GLOverlay;
-class Obstacle;
-class ObstacleList;
-class ObstacleMap;
-class Path;
-class PointCloudXYZI;
-class PointCloudXYZI_PointXYZI;
-class PointCloudXYZRGB;
-class PointCloudXYZRGB_PointXYZRGB;
-class RGBDImage;
-class Waypoint;
-
-enum GLOverlay_CoordinateFrameType {
-  GLOverlay_CoordinateFrameType_GLOBAL = 0,
-  GLOverlay_CoordinateFrameType_LOCAL = 1
-};
-bool GLOverlay_CoordinateFrameType_IsValid(int value);
-const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN = GLOverlay_CoordinateFrameType_GLOBAL;
-const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX = GLOverlay_CoordinateFrameType_LOCAL;
-const int GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE = GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX + 1;
-
-const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor();
-inline const ::std::string& GLOverlay_CoordinateFrameType_Name(GLOverlay_CoordinateFrameType value) {
-  return ::google::protobuf::internal::NameOfEnum(
-    GLOverlay_CoordinateFrameType_descriptor(), value);
-}
-inline bool GLOverlay_CoordinateFrameType_Parse(
-    const ::std::string& name, GLOverlay_CoordinateFrameType* value) {
-  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_CoordinateFrameType>(
-    GLOverlay_CoordinateFrameType_descriptor(), name, value);
-}
-enum GLOverlay_Mode {
-  GLOverlay_Mode_POINTS = 0,
-  GLOverlay_Mode_LINES = 1,
-  GLOverlay_Mode_LINE_STRIP = 2,
-  GLOverlay_Mode_LINE_LOOP = 3,
-  GLOverlay_Mode_TRIANGLES = 4,
-  GLOverlay_Mode_TRIANGLE_STRIP = 5,
-  GLOverlay_Mode_TRIANGLE_FAN = 6,
-  GLOverlay_Mode_QUADS = 7,
-  GLOverlay_Mode_QUAD_STRIP = 8,
-  GLOverlay_Mode_POLYGON = 9,
-  GLOverlay_Mode_SOLID_CIRCLE = 10,
-  GLOverlay_Mode_WIRE_CIRCLE = 11,
-  GLOverlay_Mode_SOLID_CUBE = 12,
-  GLOverlay_Mode_WIRE_CUBE = 13
-};
-bool GLOverlay_Mode_IsValid(int value);
-const GLOverlay_Mode GLOverlay_Mode_Mode_MIN = GLOverlay_Mode_POINTS;
-const GLOverlay_Mode GLOverlay_Mode_Mode_MAX = GLOverlay_Mode_WIRE_CUBE;
-const int GLOverlay_Mode_Mode_ARRAYSIZE = GLOverlay_Mode_Mode_MAX + 1;
-
-const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor();
-inline const ::std::string& GLOverlay_Mode_Name(GLOverlay_Mode value) {
-  return ::google::protobuf::internal::NameOfEnum(
-    GLOverlay_Mode_descriptor(), value);
-}
-inline bool GLOverlay_Mode_Parse(
-    const ::std::string& name, GLOverlay_Mode* value) {
-  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_Mode>(
-    GLOverlay_Mode_descriptor(), name, value);
-}
-enum GLOverlay_Identifier {
-  GLOverlay_Identifier_END = 14,
-  GLOverlay_Identifier_VERTEX2F = 15,
-  GLOverlay_Identifier_VERTEX3F = 16,
-  GLOverlay_Identifier_ROTATEF = 17,
-  GLOverlay_Identifier_TRANSLATEF = 18,
-  GLOverlay_Identifier_SCALEF = 19,
-  GLOverlay_Identifier_PUSH_MATRIX = 20,
-  GLOverlay_Identifier_POP_MATRIX = 21,
-  GLOverlay_Identifier_COLOR3F = 22,
-  GLOverlay_Identifier_COLOR4F = 23,
-  GLOverlay_Identifier_POINTSIZE = 24,
-  GLOverlay_Identifier_LINEWIDTH = 25
-};
-bool GLOverlay_Identifier_IsValid(int value);
-const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MIN = GLOverlay_Identifier_END;
-const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MAX = GLOverlay_Identifier_LINEWIDTH;
-const int GLOverlay_Identifier_Identifier_ARRAYSIZE = GLOverlay_Identifier_Identifier_MAX + 1;
-
-const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor();
-inline const ::std::string& GLOverlay_Identifier_Name(GLOverlay_Identifier value) {
-  return ::google::protobuf::internal::NameOfEnum(
-    GLOverlay_Identifier_descriptor(), value);
-}
-inline bool GLOverlay_Identifier_Parse(
-    const ::std::string& name, GLOverlay_Identifier* value) {
-  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_Identifier>(
-    GLOverlay_Identifier_descriptor(), name, value);
-}
-// ===================================================================
-
-class HeaderInfo : public ::google::protobuf::Message {
- public:
-  HeaderInfo();
-  virtual ~HeaderInfo();
-  
-  HeaderInfo(const HeaderInfo& from);
-  
-  inline HeaderInfo& operator=(const HeaderInfo& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const HeaderInfo& default_instance();
-  
-  void Swap(HeaderInfo* other);
-  
-  // implements Message ----------------------------------------------
-  
-  HeaderInfo* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const HeaderInfo& from);
-  void MergeFrom(const HeaderInfo& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required int32 source_sysid = 1;
-  inline bool has_source_sysid() const;
-  inline void clear_source_sysid();
-  static const int kSourceSysidFieldNumber = 1;
-  inline ::google::protobuf::int32 source_sysid() const;
-  inline void set_source_sysid(::google::protobuf::int32 value);
-  
-  // required int32 source_compid = 2;
-  inline bool has_source_compid() const;
-  inline void clear_source_compid();
-  static const int kSourceCompidFieldNumber = 2;
-  inline ::google::protobuf::int32 source_compid() const;
-  inline void set_source_compid(::google::protobuf::int32 value);
-  
-  // required double timestamp = 3;
-  inline bool has_timestamp() const;
-  inline void clear_timestamp();
-  static const int kTimestampFieldNumber = 3;
-  inline double timestamp() const;
-  inline void set_timestamp(double value);
-  
-  // @@protoc_insertion_point(class_scope:px.HeaderInfo)
- private:
-  inline void set_has_source_sysid();
-  inline void clear_has_source_sysid();
-  inline void set_has_source_compid();
-  inline void clear_has_source_compid();
-  inline void set_has_timestamp();
-  inline void clear_has_timestamp();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::google::protobuf::int32 source_sysid_;
-  ::google::protobuf::int32 source_compid_;
-  double timestamp_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(3 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static HeaderInfo* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class GLOverlay : public ::google::protobuf::Message {
- public:
-  GLOverlay();
-  virtual ~GLOverlay();
-  
-  GLOverlay(const GLOverlay& from);
-  
-  inline GLOverlay& operator=(const GLOverlay& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const GLOverlay& default_instance();
-  
-  void Swap(GLOverlay* other);
-  
-  // implements Message ----------------------------------------------
-  
-  GLOverlay* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const GLOverlay& from);
-  void MergeFrom(const GLOverlay& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  typedef GLOverlay_CoordinateFrameType CoordinateFrameType;
-  static const CoordinateFrameType GLOBAL = GLOverlay_CoordinateFrameType_GLOBAL;
-  static const CoordinateFrameType LOCAL = GLOverlay_CoordinateFrameType_LOCAL;
-  static inline bool CoordinateFrameType_IsValid(int value) {
-    return GLOverlay_CoordinateFrameType_IsValid(value);
-  }
-  static const CoordinateFrameType CoordinateFrameType_MIN =
-    GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN;
-  static const CoordinateFrameType CoordinateFrameType_MAX =
-    GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX;
-  static const int CoordinateFrameType_ARRAYSIZE =
-    GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE;
-  static inline const ::google::protobuf::EnumDescriptor*
-  CoordinateFrameType_descriptor() {
-    return GLOverlay_CoordinateFrameType_descriptor();
-  }
-  static inline const ::std::string& CoordinateFrameType_Name(CoordinateFrameType value) {
-    return GLOverlay_CoordinateFrameType_Name(value);
-  }
-  static inline bool CoordinateFrameType_Parse(const ::std::string& name,
-      CoordinateFrameType* value) {
-    return GLOverlay_CoordinateFrameType_Parse(name, value);
-  }
-  
-  typedef GLOverlay_Mode Mode;
-  static const Mode POINTS = GLOverlay_Mode_POINTS;
-  static const Mode LINES = GLOverlay_Mode_LINES;
-  static const Mode LINE_STRIP = GLOverlay_Mode_LINE_STRIP;
-  static const Mode LINE_LOOP = GLOverlay_Mode_LINE_LOOP;
-  static const Mode TRIANGLES = GLOverlay_Mode_TRIANGLES;
-  static const Mode TRIANGLE_STRIP = GLOverlay_Mode_TRIANGLE_STRIP;
-  static const Mode TRIANGLE_FAN = GLOverlay_Mode_TRIANGLE_FAN;
-  static const Mode QUADS = GLOverlay_Mode_QUADS;
-  static const Mode QUAD_STRIP = GLOverlay_Mode_QUAD_STRIP;
-  static const Mode POLYGON = GLOverlay_Mode_POLYGON;
-  static const Mode SOLID_CIRCLE = GLOverlay_Mode_SOLID_CIRCLE;
-  static const Mode WIRE_CIRCLE = GLOverlay_Mode_WIRE_CIRCLE;
-  static const Mode SOLID_CUBE = GLOverlay_Mode_SOLID_CUBE;
-  static const Mode WIRE_CUBE = GLOverlay_Mode_WIRE_CUBE;
-  static inline bool Mode_IsValid(int value) {
-    return GLOverlay_Mode_IsValid(value);
-  }
-  static const Mode Mode_MIN =
-    GLOverlay_Mode_Mode_MIN;
-  static const Mode Mode_MAX =
-    GLOverlay_Mode_Mode_MAX;
-  static const int Mode_ARRAYSIZE =
-    GLOverlay_Mode_Mode_ARRAYSIZE;
-  static inline const ::google::protobuf::EnumDescriptor*
-  Mode_descriptor() {
-    return GLOverlay_Mode_descriptor();
-  }
-  static inline const ::std::string& Mode_Name(Mode value) {
-    return GLOverlay_Mode_Name(value);
-  }
-  static inline bool Mode_Parse(const ::std::string& name,
-      Mode* value) {
-    return GLOverlay_Mode_Parse(name, value);
-  }
-  
-  typedef GLOverlay_Identifier Identifier;
-  static const Identifier END = GLOverlay_Identifier_END;
-  static const Identifier VERTEX2F = GLOverlay_Identifier_VERTEX2F;
-  static const Identifier VERTEX3F = GLOverlay_Identifier_VERTEX3F;
-  static const Identifier ROTATEF = GLOverlay_Identifier_ROTATEF;
-  static const Identifier TRANSLATEF = GLOverlay_Identifier_TRANSLATEF;
-  static const Identifier SCALEF = GLOverlay_Identifier_SCALEF;
-  static const Identifier PUSH_MATRIX = GLOverlay_Identifier_PUSH_MATRIX;
-  static const Identifier POP_MATRIX = GLOverlay_Identifier_POP_MATRIX;
-  static const Identifier COLOR3F = GLOverlay_Identifier_COLOR3F;
-  static const Identifier COLOR4F = GLOverlay_Identifier_COLOR4F;
-  static const Identifier POINTSIZE = GLOverlay_Identifier_POINTSIZE;
-  static const Identifier LINEWIDTH = GLOverlay_Identifier_LINEWIDTH;
-  static inline bool Identifier_IsValid(int value) {
-    return GLOverlay_Identifier_IsValid(value);
-  }
-  static const Identifier Identifier_MIN =
-    GLOverlay_Identifier_Identifier_MIN;
-  static const Identifier Identifier_MAX =
-    GLOverlay_Identifier_Identifier_MAX;
-  static const int Identifier_ARRAYSIZE =
-    GLOverlay_Identifier_Identifier_ARRAYSIZE;
-  static inline const ::google::protobuf::EnumDescriptor*
-  Identifier_descriptor() {
-    return GLOverlay_Identifier_descriptor();
-  }
-  static inline const ::std::string& Identifier_Name(Identifier value) {
-    return GLOverlay_Identifier_Name(value);
-  }
-  static inline bool Identifier_Parse(const ::std::string& name,
-      Identifier* value) {
-    return GLOverlay_Identifier_Parse(name, value);
-  }
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // optional string name = 2;
-  inline bool has_name() const;
-  inline void clear_name();
-  static const int kNameFieldNumber = 2;
-  inline const ::std::string& name() const;
-  inline void set_name(const ::std::string& value);
-  inline void set_name(const char* value);
-  inline void set_name(const char* value, size_t size);
-  inline ::std::string* mutable_name();
-  inline ::std::string* release_name();
-  
-  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-  inline bool has_coordinateframetype() const;
-  inline void clear_coordinateframetype();
-  static const int kCoordinateFrameTypeFieldNumber = 3;
-  inline ::px::GLOverlay_CoordinateFrameType coordinateframetype() const;
-  inline void set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value);
-  
-  // optional double origin_x = 4;
-  inline bool has_origin_x() const;
-  inline void clear_origin_x();
-  static const int kOriginXFieldNumber = 4;
-  inline double origin_x() const;
-  inline void set_origin_x(double value);
-  
-  // optional double origin_y = 5;
-  inline bool has_origin_y() const;
-  inline void clear_origin_y();
-  static const int kOriginYFieldNumber = 5;
-  inline double origin_y() const;
-  inline void set_origin_y(double value);
-  
-  // optional double origin_z = 6;
-  inline bool has_origin_z() const;
-  inline void clear_origin_z();
-  static const int kOriginZFieldNumber = 6;
-  inline double origin_z() const;
-  inline void set_origin_z(double value);
-  
-  // optional bytes data = 7;
-  inline bool has_data() const;
-  inline void clear_data();
-  static const int kDataFieldNumber = 7;
-  inline const ::std::string& data() const;
-  inline void set_data(const ::std::string& value);
-  inline void set_data(const char* value);
-  inline void set_data(const void* value, size_t size);
-  inline ::std::string* mutable_data();
-  inline ::std::string* release_data();
-  
-  // @@protoc_insertion_point(class_scope:px.GLOverlay)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  inline void set_has_name();
-  inline void clear_has_name();
-  inline void set_has_coordinateframetype();
-  inline void clear_has_coordinateframetype();
-  inline void set_has_origin_x();
-  inline void clear_has_origin_x();
-  inline void set_has_origin_y();
-  inline void clear_has_origin_y();
-  inline void set_has_origin_z();
-  inline void clear_has_origin_z();
-  inline void set_has_data();
-  inline void clear_has_data();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::std::string* name_;
-  double origin_x_;
-  double origin_y_;
-  double origin_z_;
-  ::std::string* data_;
-  int coordinateframetype_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(7 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static GLOverlay* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class Obstacle : public ::google::protobuf::Message {
- public:
-  Obstacle();
-  virtual ~Obstacle();
-  
-  Obstacle(const Obstacle& from);
-  
-  inline Obstacle& operator=(const Obstacle& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const Obstacle& default_instance();
-  
-  void Swap(Obstacle* other);
-  
-  // implements Message ----------------------------------------------
-  
-  Obstacle* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const Obstacle& from);
-  void MergeFrom(const Obstacle& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // optional float x = 1;
-  inline bool has_x() const;
-  inline void clear_x();
-  static const int kXFieldNumber = 1;
-  inline float x() const;
-  inline void set_x(float value);
-  
-  // optional float y = 2;
-  inline bool has_y() const;
-  inline void clear_y();
-  static const int kYFieldNumber = 2;
-  inline float y() const;
-  inline void set_y(float value);
-  
-  // optional float z = 3;
-  inline bool has_z() const;
-  inline void clear_z();
-  static const int kZFieldNumber = 3;
-  inline float z() const;
-  inline void set_z(float value);
-  
-  // optional float length = 4;
-  inline bool has_length() const;
-  inline void clear_length();
-  static const int kLengthFieldNumber = 4;
-  inline float length() const;
-  inline void set_length(float value);
-  
-  // optional float width = 5;
-  inline bool has_width() const;
-  inline void clear_width();
-  static const int kWidthFieldNumber = 5;
-  inline float width() const;
-  inline void set_width(float value);
-  
-  // optional float height = 6;
-  inline bool has_height() const;
-  inline void clear_height();
-  static const int kHeightFieldNumber = 6;
-  inline float height() const;
-  inline void set_height(float value);
-  
-  // @@protoc_insertion_point(class_scope:px.Obstacle)
- private:
-  inline void set_has_x();
-  inline void clear_has_x();
-  inline void set_has_y();
-  inline void clear_has_y();
-  inline void set_has_z();
-  inline void clear_has_z();
-  inline void set_has_length();
-  inline void clear_has_length();
-  inline void set_has_width();
-  inline void clear_has_width();
-  inline void set_has_height();
-  inline void clear_has_height();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  float x_;
-  float y_;
-  float z_;
-  float length_;
-  float width_;
-  float height_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static Obstacle* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class ObstacleList : public ::google::protobuf::Message {
- public:
-  ObstacleList();
-  virtual ~ObstacleList();
-  
-  ObstacleList(const ObstacleList& from);
-  
-  inline ObstacleList& operator=(const ObstacleList& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const ObstacleList& default_instance();
-  
-  void Swap(ObstacleList* other);
-  
-  // implements Message ----------------------------------------------
-  
-  ObstacleList* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const ObstacleList& from);
-  void MergeFrom(const ObstacleList& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.Obstacle obstacles = 2;
-  inline int obstacles_size() const;
-  inline void clear_obstacles();
-  static const int kObstaclesFieldNumber = 2;
-  inline const ::px::Obstacle& obstacles(int index) const;
-  inline ::px::Obstacle* mutable_obstacles(int index);
-  inline ::px::Obstacle* add_obstacles();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
-      obstacles() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
-      mutable_obstacles();
-  
-  // @@protoc_insertion_point(class_scope:px.ObstacleList)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static ObstacleList* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class ObstacleMap : public ::google::protobuf::Message {
- public:
-  ObstacleMap();
-  virtual ~ObstacleMap();
-  
-  ObstacleMap(const ObstacleMap& from);
-  
-  inline ObstacleMap& operator=(const ObstacleMap& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const ObstacleMap& default_instance();
-  
-  void Swap(ObstacleMap* other);
-  
-  // implements Message ----------------------------------------------
-  
-  ObstacleMap* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const ObstacleMap& from);
-  void MergeFrom(const ObstacleMap& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // required int32 type = 2;
-  inline bool has_type() const;
-  inline void clear_type();
-  static const int kTypeFieldNumber = 2;
-  inline ::google::protobuf::int32 type() const;
-  inline void set_type(::google::protobuf::int32 value);
-  
-  // optional float resolution = 3;
-  inline bool has_resolution() const;
-  inline void clear_resolution();
-  static const int kResolutionFieldNumber = 3;
-  inline float resolution() const;
-  inline void set_resolution(float value);
-  
-  // optional int32 rows = 4;
-  inline bool has_rows() const;
-  inline void clear_rows();
-  static const int kRowsFieldNumber = 4;
-  inline ::google::protobuf::int32 rows() const;
-  inline void set_rows(::google::protobuf::int32 value);
-  
-  // optional int32 cols = 5;
-  inline bool has_cols() const;
-  inline void clear_cols();
-  static const int kColsFieldNumber = 5;
-  inline ::google::protobuf::int32 cols() const;
-  inline void set_cols(::google::protobuf::int32 value);
-  
-  // optional int32 mapR0 = 6;
-  inline bool has_mapr0() const;
-  inline void clear_mapr0();
-  static const int kMapR0FieldNumber = 6;
-  inline ::google::protobuf::int32 mapr0() const;
-  inline void set_mapr0(::google::protobuf::int32 value);
-  
-  // optional int32 mapC0 = 7;
-  inline bool has_mapc0() const;
-  inline void clear_mapc0();
-  static const int kMapC0FieldNumber = 7;
-  inline ::google::protobuf::int32 mapc0() const;
-  inline void set_mapc0(::google::protobuf::int32 value);
-  
-  // optional int32 arrayR0 = 8;
-  inline bool has_arrayr0() const;
-  inline void clear_arrayr0();
-  static const int kArrayR0FieldNumber = 8;
-  inline ::google::protobuf::int32 arrayr0() const;
-  inline void set_arrayr0(::google::protobuf::int32 value);
-  
-  // optional int32 arrayC0 = 9;
-  inline bool has_arrayc0() const;
-  inline void clear_arrayc0();
-  static const int kArrayC0FieldNumber = 9;
-  inline ::google::protobuf::int32 arrayc0() const;
-  inline void set_arrayc0(::google::protobuf::int32 value);
-  
-  // optional bytes data = 10;
-  inline bool has_data() const;
-  inline void clear_data();
-  static const int kDataFieldNumber = 10;
-  inline const ::std::string& data() const;
-  inline void set_data(const ::std::string& value);
-  inline void set_data(const char* value);
-  inline void set_data(const void* value, size_t size);
-  inline ::std::string* mutable_data();
-  inline ::std::string* release_data();
-  
-  // @@protoc_insertion_point(class_scope:px.ObstacleMap)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  inline void set_has_type();
-  inline void clear_has_type();
-  inline void set_has_resolution();
-  inline void clear_has_resolution();
-  inline void set_has_rows();
-  inline void clear_has_rows();
-  inline void set_has_cols();
-  inline void clear_has_cols();
-  inline void set_has_mapr0();
-  inline void clear_has_mapr0();
-  inline void set_has_mapc0();
-  inline void clear_has_mapc0();
-  inline void set_has_arrayr0();
-  inline void clear_has_arrayr0();
-  inline void set_has_arrayc0();
-  inline void clear_has_arrayc0();
-  inline void set_has_data();
-  inline void clear_has_data();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::int32 type_;
-  float resolution_;
-  ::google::protobuf::int32 rows_;
-  ::google::protobuf::int32 cols_;
-  ::google::protobuf::int32 mapr0_;
-  ::google::protobuf::int32 mapc0_;
-  ::google::protobuf::int32 arrayr0_;
-  ::google::protobuf::int32 arrayc0_;
-  ::std::string* data_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(10 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static ObstacleMap* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class Path : public ::google::protobuf::Message {
- public:
-  Path();
-  virtual ~Path();
-  
-  Path(const Path& from);
-  
-  inline Path& operator=(const Path& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const Path& default_instance();
-  
-  void Swap(Path* other);
-  
-  // implements Message ----------------------------------------------
-  
-  Path* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const Path& from);
-  void MergeFrom(const Path& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.Waypoint waypoints = 2;
-  inline int waypoints_size() const;
-  inline void clear_waypoints();
-  static const int kWaypointsFieldNumber = 2;
-  inline const ::px::Waypoint& waypoints(int index) const;
-  inline ::px::Waypoint* mutable_waypoints(int index);
-  inline ::px::Waypoint* add_waypoints();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >&
-      waypoints() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >*
-      mutable_waypoints();
-  
-  // @@protoc_insertion_point(class_scope:px.Path)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::Waypoint > waypoints_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static Path* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message {
- public:
-  PointCloudXYZI_PointXYZI();
-  virtual ~PointCloudXYZI_PointXYZI();
-  
-  PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from);
-  
-  inline PointCloudXYZI_PointXYZI& operator=(const PointCloudXYZI_PointXYZI& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZI_PointXYZI& default_instance();
-  
-  void Swap(PointCloudXYZI_PointXYZI* other);
-  
-  // implements Message ----------------------------------------------
-  
-  PointCloudXYZI_PointXYZI* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZI_PointXYZI& from);
-  void MergeFrom(const PointCloudXYZI_PointXYZI& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required float x = 1;
-  inline bool has_x() const;
-  inline void clear_x();
-  static const int kXFieldNumber = 1;
-  inline float x() const;
-  inline void set_x(float value);
-  
-  // required float y = 2;
-  inline bool has_y() const;
-  inline void clear_y();
-  static const int kYFieldNumber = 2;
-  inline float y() const;
-  inline void set_y(float value);
-  
-  // required float z = 3;
-  inline bool has_z() const;
-  inline void clear_z();
-  static const int kZFieldNumber = 3;
-  inline float z() const;
-  inline void set_z(float value);
-  
-  // required float intensity = 4;
-  inline bool has_intensity() const;
-  inline void clear_intensity();
-  static const int kIntensityFieldNumber = 4;
-  inline float intensity() const;
-  inline void set_intensity(float value);
-  
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZI.PointXYZI)
- private:
-  inline void set_has_x();
-  inline void clear_has_x();
-  inline void set_has_y();
-  inline void clear_has_y();
-  inline void set_has_z();
-  inline void clear_has_z();
-  inline void set_has_intensity();
-  inline void clear_has_intensity();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  float x_;
-  float y_;
-  float z_;
-  float intensity_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static PointCloudXYZI_PointXYZI* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class PointCloudXYZI : public ::google::protobuf::Message {
- public:
-  PointCloudXYZI();
-  virtual ~PointCloudXYZI();
-  
-  PointCloudXYZI(const PointCloudXYZI& from);
-  
-  inline PointCloudXYZI& operator=(const PointCloudXYZI& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZI& default_instance();
-  
-  void Swap(PointCloudXYZI* other);
-  
-  // implements Message ----------------------------------------------
-  
-  PointCloudXYZI* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZI& from);
-  void MergeFrom(const PointCloudXYZI& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  typedef PointCloudXYZI_PointXYZI PointXYZI;
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  inline int points_size() const;
-  inline void clear_points();
-  static const int kPointsFieldNumber = 2;
-  inline const ::px::PointCloudXYZI_PointXYZI& points(int index) const;
-  inline ::px::PointCloudXYZI_PointXYZI* mutable_points(int index);
-  inline ::px::PointCloudXYZI_PointXYZI* add_points();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >&
-      points() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >*
-      mutable_points();
-  
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZI)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI > points_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static PointCloudXYZI* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message {
- public:
-  PointCloudXYZRGB_PointXYZRGB();
-  virtual ~PointCloudXYZRGB_PointXYZRGB();
-  
-  PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from);
-  
-  inline PointCloudXYZRGB_PointXYZRGB& operator=(const PointCloudXYZRGB_PointXYZRGB& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZRGB_PointXYZRGB& default_instance();
-  
-  void Swap(PointCloudXYZRGB_PointXYZRGB* other);
-  
-  // implements Message ----------------------------------------------
-  
-  PointCloudXYZRGB_PointXYZRGB* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from);
-  void MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required float x = 1;
-  inline bool has_x() const;
-  inline void clear_x();
-  static const int kXFieldNumber = 1;
-  inline float x() const;
-  inline void set_x(float value);
-  
-  // required float y = 2;
-  inline bool has_y() const;
-  inline void clear_y();
-  static const int kYFieldNumber = 2;
-  inline float y() const;
-  inline void set_y(float value);
-  
-  // required float z = 3;
-  inline bool has_z() const;
-  inline void clear_z();
-  static const int kZFieldNumber = 3;
-  inline float z() const;
-  inline void set_z(float value);
-  
-  // required float rgb = 4;
-  inline bool has_rgb() const;
-  inline void clear_rgb();
-  static const int kRgbFieldNumber = 4;
-  inline float rgb() const;
-  inline void set_rgb(float value);
-  
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB.PointXYZRGB)
- private:
-  inline void set_has_x();
-  inline void clear_has_x();
-  inline void set_has_y();
-  inline void clear_has_y();
-  inline void set_has_z();
-  inline void clear_has_z();
-  inline void set_has_rgb();
-  inline void clear_has_rgb();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  float x_;
-  float y_;
-  float z_;
-  float rgb_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static PointCloudXYZRGB_PointXYZRGB* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class PointCloudXYZRGB : public ::google::protobuf::Message {
- public:
-  PointCloudXYZRGB();
-  virtual ~PointCloudXYZRGB();
-  
-  PointCloudXYZRGB(const PointCloudXYZRGB& from);
-  
-  inline PointCloudXYZRGB& operator=(const PointCloudXYZRGB& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZRGB& default_instance();
-  
-  void Swap(PointCloudXYZRGB* other);
-  
-  // implements Message ----------------------------------------------
-  
-  PointCloudXYZRGB* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZRGB& from);
-  void MergeFrom(const PointCloudXYZRGB& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  typedef PointCloudXYZRGB_PointXYZRGB PointXYZRGB;
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  inline int points_size() const;
-  inline void clear_points();
-  static const int kPointsFieldNumber = 2;
-  inline const ::px::PointCloudXYZRGB_PointXYZRGB& points(int index) const;
-  inline ::px::PointCloudXYZRGB_PointXYZRGB* mutable_points(int index);
-  inline ::px::PointCloudXYZRGB_PointXYZRGB* add_points();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >&
-      points() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >*
-      mutable_points();
-  
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB > points_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static PointCloudXYZRGB* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class RGBDImage : public ::google::protobuf::Message {
- public:
-  RGBDImage();
-  virtual ~RGBDImage();
-  
-  RGBDImage(const RGBDImage& from);
-  
-  inline RGBDImage& operator=(const RGBDImage& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const RGBDImage& default_instance();
-  
-  void Swap(RGBDImage* other);
-  
-  // implements Message ----------------------------------------------
-  
-  RGBDImage* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const RGBDImage& from);
-  void MergeFrom(const RGBDImage& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // required uint32 cols = 2;
-  inline bool has_cols() const;
-  inline void clear_cols();
-  static const int kColsFieldNumber = 2;
-  inline ::google::protobuf::uint32 cols() const;
-  inline void set_cols(::google::protobuf::uint32 value);
-  
-  // required uint32 rows = 3;
-  inline bool has_rows() const;
-  inline void clear_rows();
-  static const int kRowsFieldNumber = 3;
-  inline ::google::protobuf::uint32 rows() const;
-  inline void set_rows(::google::protobuf::uint32 value);
-  
-  // required uint32 step1 = 4;
-  inline bool has_step1() const;
-  inline void clear_step1();
-  static const int kStep1FieldNumber = 4;
-  inline ::google::protobuf::uint32 step1() const;
-  inline void set_step1(::google::protobuf::uint32 value);
-  
-  // required uint32 type1 = 5;
-  inline bool has_type1() const;
-  inline void clear_type1();
-  static const int kType1FieldNumber = 5;
-  inline ::google::protobuf::uint32 type1() const;
-  inline void set_type1(::google::protobuf::uint32 value);
-  
-  // required bytes imageData1 = 6;
-  inline bool has_imagedata1() const;
-  inline void clear_imagedata1();
-  static const int kImageData1FieldNumber = 6;
-  inline const ::std::string& imagedata1() const;
-  inline void set_imagedata1(const ::std::string& value);
-  inline void set_imagedata1(const char* value);
-  inline void set_imagedata1(const void* value, size_t size);
-  inline ::std::string* mutable_imagedata1();
-  inline ::std::string* release_imagedata1();
-  
-  // required uint32 step2 = 7;
-  inline bool has_step2() const;
-  inline void clear_step2();
-  static const int kStep2FieldNumber = 7;
-  inline ::google::protobuf::uint32 step2() const;
-  inline void set_step2(::google::protobuf::uint32 value);
-  
-  // required uint32 type2 = 8;
-  inline bool has_type2() const;
-  inline void clear_type2();
-  static const int kType2FieldNumber = 8;
-  inline ::google::protobuf::uint32 type2() const;
-  inline void set_type2(::google::protobuf::uint32 value);
-  
-  // required bytes imageData2 = 9;
-  inline bool has_imagedata2() const;
-  inline void clear_imagedata2();
-  static const int kImageData2FieldNumber = 9;
-  inline const ::std::string& imagedata2() const;
-  inline void set_imagedata2(const ::std::string& value);
-  inline void set_imagedata2(const char* value);
-  inline void set_imagedata2(const void* value, size_t size);
-  inline ::std::string* mutable_imagedata2();
-  inline ::std::string* release_imagedata2();
-  
-  // optional uint32 camera_config = 10;
-  inline bool has_camera_config() const;
-  inline void clear_camera_config();
-  static const int kCameraConfigFieldNumber = 10;
-  inline ::google::protobuf::uint32 camera_config() const;
-  inline void set_camera_config(::google::protobuf::uint32 value);
-  
-  // optional uint32 camera_type = 11;
-  inline bool has_camera_type() const;
-  inline void clear_camera_type();
-  static const int kCameraTypeFieldNumber = 11;
-  inline ::google::protobuf::uint32 camera_type() const;
-  inline void set_camera_type(::google::protobuf::uint32 value);
-  
-  // optional float roll = 12;
-  inline bool has_roll() const;
-  inline void clear_roll();
-  static const int kRollFieldNumber = 12;
-  inline float roll() const;
-  inline void set_roll(float value);
-  
-  // optional float pitch = 13;
-  inline bool has_pitch() const;
-  inline void clear_pitch();
-  static const int kPitchFieldNumber = 13;
-  inline float pitch() const;
-  inline void set_pitch(float value);
-  
-  // optional float yaw = 14;
-  inline bool has_yaw() const;
-  inline void clear_yaw();
-  static const int kYawFieldNumber = 14;
-  inline float yaw() const;
-  inline void set_yaw(float value);
-  
-  // optional float lon = 15;
-  inline bool has_lon() const;
-  inline void clear_lon();
-  static const int kLonFieldNumber = 15;
-  inline float lon() const;
-  inline void set_lon(float value);
-  
-  // optional float lat = 16;
-  inline bool has_lat() const;
-  inline void clear_lat();
-  static const int kLatFieldNumber = 16;
-  inline float lat() const;
-  inline void set_lat(float value);
-  
-  // optional float alt = 17;
-  inline bool has_alt() const;
-  inline void clear_alt();
-  static const int kAltFieldNumber = 17;
-  inline float alt() const;
-  inline void set_alt(float value);
-  
-  // optional float ground_x = 18;
-  inline bool has_ground_x() const;
-  inline void clear_ground_x();
-  static const int kGroundXFieldNumber = 18;
-  inline float ground_x() const;
-  inline void set_ground_x(float value);
-  
-  // optional float ground_y = 19;
-  inline bool has_ground_y() const;
-  inline void clear_ground_y();
-  static const int kGroundYFieldNumber = 19;
-  inline float ground_y() const;
-  inline void set_ground_y(float value);
-  
-  // optional float ground_z = 20;
-  inline bool has_ground_z() const;
-  inline void clear_ground_z();
-  static const int kGroundZFieldNumber = 20;
-  inline float ground_z() const;
-  inline void set_ground_z(float value);
-  
-  // repeated float camera_matrix = 21;
-  inline int camera_matrix_size() const;
-  inline void clear_camera_matrix();
-  static const int kCameraMatrixFieldNumber = 21;
-  inline float camera_matrix(int index) const;
-  inline void set_camera_matrix(int index, float value);
-  inline void add_camera_matrix(float value);
-  inline const ::google::protobuf::RepeatedField< float >&
-      camera_matrix() const;
-  inline ::google::protobuf::RepeatedField< float >*
-      mutable_camera_matrix();
-  
-  // @@protoc_insertion_point(class_scope:px.RGBDImage)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  inline void set_has_cols();
-  inline void clear_has_cols();
-  inline void set_has_rows();
-  inline void clear_has_rows();
-  inline void set_has_step1();
-  inline void clear_has_step1();
-  inline void set_has_type1();
-  inline void clear_has_type1();
-  inline void set_has_imagedata1();
-  inline void clear_has_imagedata1();
-  inline void set_has_step2();
-  inline void clear_has_step2();
-  inline void set_has_type2();
-  inline void clear_has_type2();
-  inline void set_has_imagedata2();
-  inline void clear_has_imagedata2();
-  inline void set_has_camera_config();
-  inline void clear_has_camera_config();
-  inline void set_has_camera_type();
-  inline void clear_has_camera_type();
-  inline void set_has_roll();
-  inline void clear_has_roll();
-  inline void set_has_pitch();
-  inline void clear_has_pitch();
-  inline void set_has_yaw();
-  inline void clear_has_yaw();
-  inline void set_has_lon();
-  inline void clear_has_lon();
-  inline void set_has_lat();
-  inline void clear_has_lat();
-  inline void set_has_alt();
-  inline void clear_has_alt();
-  inline void set_has_ground_x();
-  inline void clear_has_ground_x();
-  inline void set_has_ground_y();
-  inline void clear_has_ground_y();
-  inline void set_has_ground_z();
-  inline void clear_has_ground_z();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::uint32 cols_;
-  ::google::protobuf::uint32 rows_;
-  ::google::protobuf::uint32 step1_;
-  ::google::protobuf::uint32 type1_;
-  ::std::string* imagedata1_;
-  ::google::protobuf::uint32 step2_;
-  ::google::protobuf::uint32 type2_;
-  ::std::string* imagedata2_;
-  ::google::protobuf::uint32 camera_config_;
-  ::google::protobuf::uint32 camera_type_;
-  float roll_;
-  float pitch_;
-  float yaw_;
-  float lon_;
-  float lat_;
-  float alt_;
-  float ground_x_;
-  float ground_y_;
-  ::google::protobuf::RepeatedField< float > camera_matrix_;
-  float ground_z_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(21 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static RGBDImage* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class Waypoint : public ::google::protobuf::Message {
- public:
-  Waypoint();
-  virtual ~Waypoint();
-  
-  Waypoint(const Waypoint& from);
-  
-  inline Waypoint& operator=(const Waypoint& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const Waypoint& default_instance();
-  
-  void Swap(Waypoint* other);
-  
-  // implements Message ----------------------------------------------
-  
-  Waypoint* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const Waypoint& from);
-  void MergeFrom(const Waypoint& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required double x = 1;
-  inline bool has_x() const;
-  inline void clear_x();
-  static const int kXFieldNumber = 1;
-  inline double x() const;
-  inline void set_x(double value);
-  
-  // required double y = 2;
-  inline bool has_y() const;
-  inline void clear_y();
-  static const int kYFieldNumber = 2;
-  inline double y() const;
-  inline void set_y(double value);
-  
-  // optional double z = 3;
-  inline bool has_z() const;
-  inline void clear_z();
-  static const int kZFieldNumber = 3;
-  inline double z() const;
-  inline void set_z(double value);
-  
-  // optional double roll = 4;
-  inline bool has_roll() const;
-  inline void clear_roll();
-  static const int kRollFieldNumber = 4;
-  inline double roll() const;
-  inline void set_roll(double value);
-  
-  // optional double pitch = 5;
-  inline bool has_pitch() const;
-  inline void clear_pitch();
-  static const int kPitchFieldNumber = 5;
-  inline double pitch() const;
-  inline void set_pitch(double value);
-  
-  // optional double yaw = 6;
-  inline bool has_yaw() const;
-  inline void clear_yaw();
-  static const int kYawFieldNumber = 6;
-  inline double yaw() const;
-  inline void set_yaw(double value);
-  
-  // @@protoc_insertion_point(class_scope:px.Waypoint)
- private:
-  inline void set_has_x();
-  inline void clear_has_x();
-  inline void set_has_y();
-  inline void clear_has_y();
-  inline void set_has_z();
-  inline void clear_has_z();
-  inline void set_has_roll();
-  inline void clear_has_roll();
-  inline void set_has_pitch();
-  inline void clear_has_pitch();
-  inline void set_has_yaw();
-  inline void clear_has_yaw();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  double x_;
-  double y_;
-  double z_;
-  double roll_;
-  double pitch_;
-  double yaw_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static Waypoint* default_instance_;
-};
-// ===================================================================
-
-
-// ===================================================================
-
-// HeaderInfo
-
-// required int32 source_sysid = 1;
-inline bool HeaderInfo::has_source_sysid() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void HeaderInfo::set_has_source_sysid() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void HeaderInfo::clear_has_source_sysid() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void HeaderInfo::clear_source_sysid() {
-  source_sysid_ = 0;
-  clear_has_source_sysid();
-}
-inline ::google::protobuf::int32 HeaderInfo::source_sysid() const {
-  return source_sysid_;
-}
-inline void HeaderInfo::set_source_sysid(::google::protobuf::int32 value) {
-  set_has_source_sysid();
-  source_sysid_ = value;
-}
-
-// required int32 source_compid = 2;
-inline bool HeaderInfo::has_source_compid() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void HeaderInfo::set_has_source_compid() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void HeaderInfo::clear_has_source_compid() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void HeaderInfo::clear_source_compid() {
-  source_compid_ = 0;
-  clear_has_source_compid();
-}
-inline ::google::protobuf::int32 HeaderInfo::source_compid() const {
-  return source_compid_;
-}
-inline void HeaderInfo::set_source_compid(::google::protobuf::int32 value) {
-  set_has_source_compid();
-  source_compid_ = value;
-}
-
-// required double timestamp = 3;
-inline bool HeaderInfo::has_timestamp() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void HeaderInfo::set_has_timestamp() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void HeaderInfo::clear_has_timestamp() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void HeaderInfo::clear_timestamp() {
-  timestamp_ = 0;
-  clear_has_timestamp();
-}
-inline double HeaderInfo::timestamp() const {
-  return timestamp_;
-}
-inline void HeaderInfo::set_timestamp(double value) {
-  set_has_timestamp();
-  timestamp_ = value;
-}
-
-// -------------------------------------------------------------------
-
-// GLOverlay
-
-// required .px.HeaderInfo header = 1;
-inline bool GLOverlay::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void GLOverlay::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void GLOverlay::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void GLOverlay::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& GLOverlay::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* GLOverlay::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* GLOverlay::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// optional string name = 2;
-inline bool GLOverlay::has_name() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void GLOverlay::set_has_name() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void GLOverlay::clear_has_name() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void GLOverlay::clear_name() {
-  if (name_ != &::google::protobuf::internal::kEmptyString) {
-    name_->clear();
-  }
-  clear_has_name();
-}
-inline const ::std::string& GLOverlay::name() const {
-  return *name_;
-}
-inline void GLOverlay::set_name(const ::std::string& value) {
-  set_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    name_ = new ::std::string;
-  }
-  name_->assign(value);
-}
-inline void GLOverlay::set_name(const char* value) {
-  set_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    name_ = new ::std::string;
-  }
-  name_->assign(value);
-}
-inline void GLOverlay::set_name(const char* value, size_t size) {
-  set_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    name_ = new ::std::string;
-  }
-  name_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* GLOverlay::mutable_name() {
-  set_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    name_ = new ::std::string;
-  }
-  return name_;
-}
-inline ::std::string* GLOverlay::release_name() {
-  clear_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = name_;
-    name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-inline bool GLOverlay::has_coordinateframetype() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void GLOverlay::set_has_coordinateframetype() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void GLOverlay::clear_has_coordinateframetype() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void GLOverlay::clear_coordinateframetype() {
-  coordinateframetype_ = 0;
-  clear_has_coordinateframetype();
-}
-inline ::px::GLOverlay_CoordinateFrameType GLOverlay::coordinateframetype() const {
-  return static_cast< ::px::GLOverlay_CoordinateFrameType >(coordinateframetype_);
-}
-inline void GLOverlay::set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value) {
-  GOOGLE_DCHECK(::px::GLOverlay_CoordinateFrameType_IsValid(value));
-  set_has_coordinateframetype();
-  coordinateframetype_ = value;
-}
-
-// optional double origin_x = 4;
-inline bool GLOverlay::has_origin_x() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void GLOverlay::set_has_origin_x() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void GLOverlay::clear_has_origin_x() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void GLOverlay::clear_origin_x() {
-  origin_x_ = 0;
-  clear_has_origin_x();
-}
-inline double GLOverlay::origin_x() const {
-  return origin_x_;
-}
-inline void GLOverlay::set_origin_x(double value) {
-  set_has_origin_x();
-  origin_x_ = value;
-}
-
-// optional double origin_y = 5;
-inline bool GLOverlay::has_origin_y() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void GLOverlay::set_has_origin_y() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void GLOverlay::clear_has_origin_y() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void GLOverlay::clear_origin_y() {
-  origin_y_ = 0;
-  clear_has_origin_y();
-}
-inline double GLOverlay::origin_y() const {
-  return origin_y_;
-}
-inline void GLOverlay::set_origin_y(double value) {
-  set_has_origin_y();
-  origin_y_ = value;
-}
-
-// optional double origin_z = 6;
-inline bool GLOverlay::has_origin_z() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void GLOverlay::set_has_origin_z() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void GLOverlay::clear_has_origin_z() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void GLOverlay::clear_origin_z() {
-  origin_z_ = 0;
-  clear_has_origin_z();
-}
-inline double GLOverlay::origin_z() const {
-  return origin_z_;
-}
-inline void GLOverlay::set_origin_z(double value) {
-  set_has_origin_z();
-  origin_z_ = value;
-}
-
-// optional bytes data = 7;
-inline bool GLOverlay::has_data() const {
-  return (_has_bits_[0] & 0x00000040u) != 0;
-}
-inline void GLOverlay::set_has_data() {
-  _has_bits_[0] |= 0x00000040u;
-}
-inline void GLOverlay::clear_has_data() {
-  _has_bits_[0] &= ~0x00000040u;
-}
-inline void GLOverlay::clear_data() {
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    data_->clear();
-  }
-  clear_has_data();
-}
-inline const ::std::string& GLOverlay::data() const {
-  return *data_;
-}
-inline void GLOverlay::set_data(const ::std::string& value) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(value);
-}
-inline void GLOverlay::set_data(const char* value) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(value);
-}
-inline void GLOverlay::set_data(const void* value, size_t size) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* GLOverlay::mutable_data() {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  return data_;
-}
-inline ::std::string* GLOverlay::release_data() {
-  clear_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = data_;
-    data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// -------------------------------------------------------------------
-
-// Obstacle
-
-// optional float x = 1;
-inline bool Obstacle::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void Obstacle::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void Obstacle::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void Obstacle::clear_x() {
-  x_ = 0;
-  clear_has_x();
-}
-inline float Obstacle::x() const {
-  return x_;
-}
-inline void Obstacle::set_x(float value) {
-  set_has_x();
-  x_ = value;
-}
-
-// optional float y = 2;
-inline bool Obstacle::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void Obstacle::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void Obstacle::clear_has_y() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void Obstacle::clear_y() {
-  y_ = 0;
-  clear_has_y();
-}
-inline float Obstacle::y() const {
-  return y_;
-}
-inline void Obstacle::set_y(float value) {
-  set_has_y();
-  y_ = value;
-}
-
-// optional float z = 3;
-inline bool Obstacle::has_z() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void Obstacle::set_has_z() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void Obstacle::clear_has_z() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void Obstacle::clear_z() {
-  z_ = 0;
-  clear_has_z();
-}
-inline float Obstacle::z() const {
-  return z_;
-}
-inline void Obstacle::set_z(float value) {
-  set_has_z();
-  z_ = value;
-}
-
-// optional float length = 4;
-inline bool Obstacle::has_length() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void Obstacle::set_has_length() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void Obstacle::clear_has_length() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void Obstacle::clear_length() {
-  length_ = 0;
-  clear_has_length();
-}
-inline float Obstacle::length() const {
-  return length_;
-}
-inline void Obstacle::set_length(float value) {
-  set_has_length();
-  length_ = value;
-}
-
-// optional float width = 5;
-inline bool Obstacle::has_width() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void Obstacle::set_has_width() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void Obstacle::clear_has_width() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void Obstacle::clear_width() {
-  width_ = 0;
-  clear_has_width();
-}
-inline float Obstacle::width() const {
-  return width_;
-}
-inline void Obstacle::set_width(float value) {
-  set_has_width();
-  width_ = value;
-}
-
-// optional float height = 6;
-inline bool Obstacle::has_height() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void Obstacle::set_has_height() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void Obstacle::clear_has_height() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void Obstacle::clear_height() {
-  height_ = 0;
-  clear_has_height();
-}
-inline float Obstacle::height() const {
-  return height_;
-}
-inline void Obstacle::set_height(float value) {
-  set_has_height();
-  height_ = value;
-}
-
-// -------------------------------------------------------------------
-
-// ObstacleList
-
-// required .px.HeaderInfo header = 1;
-inline bool ObstacleList::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void ObstacleList::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void ObstacleList::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void ObstacleList::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& ObstacleList::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* ObstacleList::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* ObstacleList::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.Obstacle obstacles = 2;
-inline int ObstacleList::obstacles_size() const {
-  return obstacles_.size();
-}
-inline void ObstacleList::clear_obstacles() {
-  obstacles_.Clear();
-}
-inline const ::px::Obstacle& ObstacleList::obstacles(int index) const {
-  return obstacles_.Get(index);
-}
-inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) {
-  return obstacles_.Mutable(index);
-}
-inline ::px::Obstacle* ObstacleList::add_obstacles() {
-  return obstacles_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
-ObstacleList::obstacles() const {
-  return obstacles_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
-ObstacleList::mutable_obstacles() {
-  return &obstacles_;
-}
-
-// -------------------------------------------------------------------
-
-// ObstacleMap
-
-// required .px.HeaderInfo header = 1;
-inline bool ObstacleMap::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void ObstacleMap::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void ObstacleMap::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void ObstacleMap::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& ObstacleMap::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* ObstacleMap::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* ObstacleMap::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// required int32 type = 2;
-inline bool ObstacleMap::has_type() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void ObstacleMap::set_has_type() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void ObstacleMap::clear_has_type() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void ObstacleMap::clear_type() {
-  type_ = 0;
-  clear_has_type();
-}
-inline ::google::protobuf::int32 ObstacleMap::type() const {
-  return type_;
-}
-inline void ObstacleMap::set_type(::google::protobuf::int32 value) {
-  set_has_type();
-  type_ = value;
-}
-
-// optional float resolution = 3;
-inline bool ObstacleMap::has_resolution() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void ObstacleMap::set_has_resolution() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void ObstacleMap::clear_has_resolution() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void ObstacleMap::clear_resolution() {
-  resolution_ = 0;
-  clear_has_resolution();
-}
-inline float ObstacleMap::resolution() const {
-  return resolution_;
-}
-inline void ObstacleMap::set_resolution(float value) {
-  set_has_resolution();
-  resolution_ = value;
-}
-
-// optional int32 rows = 4;
-inline bool ObstacleMap::has_rows() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void ObstacleMap::set_has_rows() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void ObstacleMap::clear_has_rows() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void ObstacleMap::clear_rows() {
-  rows_ = 0;
-  clear_has_rows();
-}
-inline ::google::protobuf::int32 ObstacleMap::rows() const {
-  return rows_;
-}
-inline void ObstacleMap::set_rows(::google::protobuf::int32 value) {
-  set_has_rows();
-  rows_ = value;
-}
-
-// optional int32 cols = 5;
-inline bool ObstacleMap::has_cols() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void ObstacleMap::set_has_cols() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void ObstacleMap::clear_has_cols() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void ObstacleMap::clear_cols() {
-  cols_ = 0;
-  clear_has_cols();
-}
-inline ::google::protobuf::int32 ObstacleMap::cols() const {
-  return cols_;
-}
-inline void ObstacleMap::set_cols(::google::protobuf::int32 value) {
-  set_has_cols();
-  cols_ = value;
-}
-
-// optional int32 mapR0 = 6;
-inline bool ObstacleMap::has_mapr0() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void ObstacleMap::set_has_mapr0() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void ObstacleMap::clear_has_mapr0() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void ObstacleMap::clear_mapr0() {
-  mapr0_ = 0;
-  clear_has_mapr0();
-}
-inline ::google::protobuf::int32 ObstacleMap::mapr0() const {
-  return mapr0_;
-}
-inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) {
-  set_has_mapr0();
-  mapr0_ = value;
-}
-
-// optional int32 mapC0 = 7;
-inline bool ObstacleMap::has_mapc0() const {
-  return (_has_bits_[0] & 0x00000040u) != 0;
-}
-inline void ObstacleMap::set_has_mapc0() {
-  _has_bits_[0] |= 0x00000040u;
-}
-inline void ObstacleMap::clear_has_mapc0() {
-  _has_bits_[0] &= ~0x00000040u;
-}
-inline void ObstacleMap::clear_mapc0() {
-  mapc0_ = 0;
-  clear_has_mapc0();
-}
-inline ::google::protobuf::int32 ObstacleMap::mapc0() const {
-  return mapc0_;
-}
-inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) {
-  set_has_mapc0();
-  mapc0_ = value;
-}
-
-// optional int32 arrayR0 = 8;
-inline bool ObstacleMap::has_arrayr0() const {
-  return (_has_bits_[0] & 0x00000080u) != 0;
-}
-inline void ObstacleMap::set_has_arrayr0() {
-  _has_bits_[0] |= 0x00000080u;
-}
-inline void ObstacleMap::clear_has_arrayr0() {
-  _has_bits_[0] &= ~0x00000080u;
-}
-inline void ObstacleMap::clear_arrayr0() {
-  arrayr0_ = 0;
-  clear_has_arrayr0();
-}
-inline ::google::protobuf::int32 ObstacleMap::arrayr0() const {
-  return arrayr0_;
-}
-inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) {
-  set_has_arrayr0();
-  arrayr0_ = value;
-}
-
-// optional int32 arrayC0 = 9;
-inline bool ObstacleMap::has_arrayc0() const {
-  return (_has_bits_[0] & 0x00000100u) != 0;
-}
-inline void ObstacleMap::set_has_arrayc0() {
-  _has_bits_[0] |= 0x00000100u;
-}
-inline void ObstacleMap::clear_has_arrayc0() {
-  _has_bits_[0] &= ~0x00000100u;
-}
-inline void ObstacleMap::clear_arrayc0() {
-  arrayc0_ = 0;
-  clear_has_arrayc0();
-}
-inline ::google::protobuf::int32 ObstacleMap::arrayc0() const {
-  return arrayc0_;
-}
-inline void ObstacleMap::set_arrayc0(::google::protobuf::int32 value) {
-  set_has_arrayc0();
-  arrayc0_ = value;
-}
-
-// optional bytes data = 10;
-inline bool ObstacleMap::has_data() const {
-  return (_has_bits_[0] & 0x00000200u) != 0;
-}
-inline void ObstacleMap::set_has_data() {
-  _has_bits_[0] |= 0x00000200u;
-}
-inline void ObstacleMap::clear_has_data() {
-  _has_bits_[0] &= ~0x00000200u;
-}
-inline void ObstacleMap::clear_data() {
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    data_->clear();
-  }
-  clear_has_data();
-}
-inline const ::std::string& ObstacleMap::data() const {
-  return *data_;
-}
-inline void ObstacleMap::set_data(const ::std::string& value) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(value);
-}
-inline void ObstacleMap::set_data(const char* value) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(value);
-}
-inline void ObstacleMap::set_data(const void* value, size_t size) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* ObstacleMap::mutable_data() {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  return data_;
-}
-inline ::std::string* ObstacleMap::release_data() {
-  clear_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = data_;
-    data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// -------------------------------------------------------------------
-
-// Path
-
-// required .px.HeaderInfo header = 1;
-inline bool Path::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void Path::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void Path::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void Path::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& Path::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* Path::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* Path::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.Waypoint waypoints = 2;
-inline int Path::waypoints_size() const {
-  return waypoints_.size();
-}
-inline void Path::clear_waypoints() {
-  waypoints_.Clear();
-}
-inline const ::px::Waypoint& Path::waypoints(int index) const {
-  return waypoints_.Get(index);
-}
-inline ::px::Waypoint* Path::mutable_waypoints(int index) {
-  return waypoints_.Mutable(index);
-}
-inline ::px::Waypoint* Path::add_waypoints() {
-  return waypoints_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >&
-Path::waypoints() const {
-  return waypoints_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >*
-Path::mutable_waypoints() {
-  return &waypoints_;
-}
-
-// -------------------------------------------------------------------
-
-// PointCloudXYZI_PointXYZI
-
-// required float x = 1;
-inline bool PointCloudXYZI_PointXYZI::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZI_PointXYZI::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_x() {
-  x_ = 0;
-  clear_has_x();
-}
-inline float PointCloudXYZI_PointXYZI::x() const {
-  return x_;
-}
-inline void PointCloudXYZI_PointXYZI::set_x(float value) {
-  set_has_x();
-  x_ = value;
-}
-
-// required float y = 2;
-inline bool PointCloudXYZI_PointXYZI::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void PointCloudXYZI_PointXYZI::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_has_y() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_y() {
-  y_ = 0;
-  clear_has_y();
-}
-inline float PointCloudXYZI_PointXYZI::y() const {
-  return y_;
-}
-inline void PointCloudXYZI_PointXYZI::set_y(float value) {
-  set_has_y();
-  y_ = value;
-}
-
-// required float z = 3;
-inline bool PointCloudXYZI_PointXYZI::has_z() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void PointCloudXYZI_PointXYZI::set_has_z() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_has_z() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_z() {
-  z_ = 0;
-  clear_has_z();
-}
-inline float PointCloudXYZI_PointXYZI::z() const {
-  return z_;
-}
-inline void PointCloudXYZI_PointXYZI::set_z(float value) {
-  set_has_z();
-  z_ = value;
-}
-
-// required float intensity = 4;
-inline bool PointCloudXYZI_PointXYZI::has_intensity() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void PointCloudXYZI_PointXYZI::set_has_intensity() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_has_intensity() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_intensity() {
-  intensity_ = 0;
-  clear_has_intensity();
-}
-inline float PointCloudXYZI_PointXYZI::intensity() const {
-  return intensity_;
-}
-inline void PointCloudXYZI_PointXYZI::set_intensity(float value) {
-  set_has_intensity();
-  intensity_ = value;
-}
-
-// -------------------------------------------------------------------
-
-// PointCloudXYZI
-
-// required .px.HeaderInfo header = 1;
-inline bool PointCloudXYZI::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZI::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void PointCloudXYZI::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void PointCloudXYZI::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& PointCloudXYZI::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* PointCloudXYZI::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* PointCloudXYZI::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.PointCloudXYZI.PointXYZI points = 2;
-inline int PointCloudXYZI::points_size() const {
-  return points_.size();
-}
-inline void PointCloudXYZI::clear_points() {
-  points_.Clear();
-}
-inline const ::px::PointCloudXYZI_PointXYZI& PointCloudXYZI::points(int index) const {
-  return points_.Get(index);
-}
-inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::mutable_points(int index) {
-  return points_.Mutable(index);
-}
-inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::add_points() {
-  return points_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >&
-PointCloudXYZI::points() const {
-  return points_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >*
-PointCloudXYZI::mutable_points() {
-  return &points_;
-}
-
-// -------------------------------------------------------------------
-
-// PointCloudXYZRGB_PointXYZRGB
-
-// required float x = 1;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_x() {
-  x_ = 0;
-  clear_has_x();
-}
-inline float PointCloudXYZRGB_PointXYZRGB::x() const {
-  return x_;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_x(float value) {
-  set_has_x();
-  x_ = value;
-}
-
-// required float y = 2;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_y() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_y() {
-  y_ = 0;
-  clear_has_y();
-}
-inline float PointCloudXYZRGB_PointXYZRGB::y() const {
-  return y_;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_y(float value) {
-  set_has_y();
-  y_ = value;
-}
-
-// required float z = 3;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_z() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_z() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_z() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_z() {
-  z_ = 0;
-  clear_has_z();
-}
-inline float PointCloudXYZRGB_PointXYZRGB::z() const {
-  return z_;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_z(float value) {
-  set_has_z();
-  z_ = value;
-}
-
-// required float rgb = 4;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_rgb() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_rgb() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_rgb() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_rgb() {
-  rgb_ = 0;
-  clear_has_rgb();
-}
-inline float PointCloudXYZRGB_PointXYZRGB::rgb() const {
-  return rgb_;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_rgb(float value) {
-  set_has_rgb();
-  rgb_ = value;
-}
-
-// -------------------------------------------------------------------
-
-// PointCloudXYZRGB
-
-// required .px.HeaderInfo header = 1;
-inline bool PointCloudXYZRGB::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZRGB::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void PointCloudXYZRGB::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void PointCloudXYZRGB::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& PointCloudXYZRGB::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* PointCloudXYZRGB::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* PointCloudXYZRGB::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-inline int PointCloudXYZRGB::points_size() const {
-  return points_.size();
-}
-inline void PointCloudXYZRGB::clear_points() {
-  points_.Clear();
-}
-inline const ::px::PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB::points(int index) const {
-  return points_.Get(index);
-}
-inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::mutable_points(int index) {
-  return points_.Mutable(index);
-}
-inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::add_points() {
-  return points_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >&
-PointCloudXYZRGB::points() const {
-  return points_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >*
-PointCloudXYZRGB::mutable_points() {
-  return &points_;
-}
-
-// -------------------------------------------------------------------
-
-// RGBDImage
-
-// required .px.HeaderInfo header = 1;
-inline bool RGBDImage::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void RGBDImage::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void RGBDImage::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void RGBDImage::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& RGBDImage::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* RGBDImage::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* RGBDImage::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// required uint32 cols = 2;
-inline bool RGBDImage::has_cols() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void RGBDImage::set_has_cols() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void RGBDImage::clear_has_cols() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void RGBDImage::clear_cols() {
-  cols_ = 0u;
-  clear_has_cols();
-}
-inline ::google::protobuf::uint32 RGBDImage::cols() const {
-  return cols_;
-}
-inline void RGBDImage::set_cols(::google::protobuf::uint32 value) {
-  set_has_cols();
-  cols_ = value;
-}
-
-// required uint32 rows = 3;
-inline bool RGBDImage::has_rows() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void RGBDImage::set_has_rows() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void RGBDImage::clear_has_rows() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void RGBDImage::clear_rows() {
-  rows_ = 0u;
-  clear_has_rows();
-}
-inline ::google::protobuf::uint32 RGBDImage::rows() const {
-  return rows_;
-}
-inline void RGBDImage::set_rows(::google::protobuf::uint32 value) {
-  set_has_rows();
-  rows_ = value;
-}
-
-// required uint32 step1 = 4;
-inline bool RGBDImage::has_step1() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void RGBDImage::set_has_step1() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void RGBDImage::clear_has_step1() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void RGBDImage::clear_step1() {
-  step1_ = 0u;
-  clear_has_step1();
-}
-inline ::google::protobuf::uint32 RGBDImage::step1() const {
-  return step1_;
-}
-inline void RGBDImage::set_step1(::google::protobuf::uint32 value) {
-  set_has_step1();
-  step1_ = value;
-}
-
-// required uint32 type1 = 5;
-inline bool RGBDImage::has_type1() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void RGBDImage::set_has_type1() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void RGBDImage::clear_has_type1() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void RGBDImage::clear_type1() {
-  type1_ = 0u;
-  clear_has_type1();
-}
-inline ::google::protobuf::uint32 RGBDImage::type1() const {
-  return type1_;
-}
-inline void RGBDImage::set_type1(::google::protobuf::uint32 value) {
-  set_has_type1();
-  type1_ = value;
-}
-
-// required bytes imageData1 = 6;
-inline bool RGBDImage::has_imagedata1() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void RGBDImage::set_has_imagedata1() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void RGBDImage::clear_has_imagedata1() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void RGBDImage::clear_imagedata1() {
-  if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-    imagedata1_->clear();
-  }
-  clear_has_imagedata1();
-}
-inline const ::std::string& RGBDImage::imagedata1() const {
-  return *imagedata1_;
-}
-inline void RGBDImage::set_imagedata1(const ::std::string& value) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(value);
-}
-inline void RGBDImage::set_imagedata1(const char* value) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(value);
-}
-inline void RGBDImage::set_imagedata1(const void* value, size_t size) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* RGBDImage::mutable_imagedata1() {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  return imagedata1_;
-}
-inline ::std::string* RGBDImage::release_imagedata1() {
-  clear_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = imagedata1_;
-    imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// required uint32 step2 = 7;
-inline bool RGBDImage::has_step2() const {
-  return (_has_bits_[0] & 0x00000040u) != 0;
-}
-inline void RGBDImage::set_has_step2() {
-  _has_bits_[0] |= 0x00000040u;
-}
-inline void RGBDImage::clear_has_step2() {
-  _has_bits_[0] &= ~0x00000040u;
-}
-inline void RGBDImage::clear_step2() {
-  step2_ = 0u;
-  clear_has_step2();
-}
-inline ::google::protobuf::uint32 RGBDImage::step2() const {
-  return step2_;
-}
-inline void RGBDImage::set_step2(::google::protobuf::uint32 value) {
-  set_has_step2();
-  step2_ = value;
-}
-
-// required uint32 type2 = 8;
-inline bool RGBDImage::has_type2() const {
-  return (_has_bits_[0] & 0x00000080u) != 0;
-}
-inline void RGBDImage::set_has_type2() {
-  _has_bits_[0] |= 0x00000080u;
-}
-inline void RGBDImage::clear_has_type2() {
-  _has_bits_[0] &= ~0x00000080u;
-}
-inline void RGBDImage::clear_type2() {
-  type2_ = 0u;
-  clear_has_type2();
-}
-inline ::google::protobuf::uint32 RGBDImage::type2() const {
-  return type2_;
-}
-inline void RGBDImage::set_type2(::google::protobuf::uint32 value) {
-  set_has_type2();
-  type2_ = value;
-}
-
-// required bytes imageData2 = 9;
-inline bool RGBDImage::has_imagedata2() const {
-  return (_has_bits_[0] & 0x00000100u) != 0;
-}
-inline void RGBDImage::set_has_imagedata2() {
-  _has_bits_[0] |= 0x00000100u;
-}
-inline void RGBDImage::clear_has_imagedata2() {
-  _has_bits_[0] &= ~0x00000100u;
-}
-inline void RGBDImage::clear_imagedata2() {
-  if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-    imagedata2_->clear();
-  }
-  clear_has_imagedata2();
-}
-inline const ::std::string& RGBDImage::imagedata2() const {
-  return *imagedata2_;
-}
-inline void RGBDImage::set_imagedata2(const ::std::string& value) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
-  }
-  imagedata2_->assign(value);
-}
-inline void RGBDImage::set_imagedata2(const char* value) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
-  }
-  imagedata2_->assign(value);
-}
-inline void RGBDImage::set_imagedata2(const void* value, size_t size) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
-  }
-  imagedata2_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* RGBDImage::mutable_imagedata2() {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
-  }
-  return imagedata2_;
-}
-inline ::std::string* RGBDImage::release_imagedata2() {
-  clear_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = imagedata2_;
-    imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// optional uint32 camera_config = 10;
-inline bool RGBDImage::has_camera_config() const {
-  return (_has_bits_[0] & 0x00000200u) != 0;
-}
-inline void RGBDImage::set_has_camera_config() {
-  _has_bits_[0] |= 0x00000200u;
-}
-inline void RGBDImage::clear_has_camera_config() {
-  _has_bits_[0] &= ~0x00000200u;
-}
-inline void RGBDImage::clear_camera_config() {
-  camera_config_ = 0u;
-  clear_has_camera_config();
-}
-inline ::google::protobuf::uint32 RGBDImage::camera_config() const {
-  return camera_config_;
-}
-inline void RGBDImage::set_camera_config(::google::protobuf::uint32 value) {
-  set_has_camera_config();
-  camera_config_ = value;
-}
-
-// optional uint32 camera_type = 11;
-inline bool RGBDImage::has_camera_type() const {
-  return (_has_bits_[0] & 0x00000400u) != 0;
-}
-inline void RGBDImage::set_has_camera_type() {
-  _has_bits_[0] |= 0x00000400u;
-}
-inline void RGBDImage::clear_has_camera_type() {
-  _has_bits_[0] &= ~0x00000400u;
-}
-inline void RGBDImage::clear_camera_type() {
-  camera_type_ = 0u;
-  clear_has_camera_type();
-}
-inline ::google::protobuf::uint32 RGBDImage::camera_type() const {
-  return camera_type_;
-}
-inline void RGBDImage::set_camera_type(::google::protobuf::uint32 value) {
-  set_has_camera_type();
-  camera_type_ = value;
-}
-
-// optional float roll = 12;
-inline bool RGBDImage::has_roll() const {
-  return (_has_bits_[0] & 0x00000800u) != 0;
-}
-inline void RGBDImage::set_has_roll() {
-  _has_bits_[0] |= 0x00000800u;
-}
-inline void RGBDImage::clear_has_roll() {
-  _has_bits_[0] &= ~0x00000800u;
-}
-inline void RGBDImage::clear_roll() {
-  roll_ = 0;
-  clear_has_roll();
-}
-inline float RGBDImage::roll() const {
-  return roll_;
-}
-inline void RGBDImage::set_roll(float value) {
-  set_has_roll();
-  roll_ = value;
-}
-
-// optional float pitch = 13;
-inline bool RGBDImage::has_pitch() const {
-  return (_has_bits_[0] & 0x00001000u) != 0;
-}
-inline void RGBDImage::set_has_pitch() {
-  _has_bits_[0] |= 0x00001000u;
-}
-inline void RGBDImage::clear_has_pitch() {
-  _has_bits_[0] &= ~0x00001000u;
-}
-inline void RGBDImage::clear_pitch() {
-  pitch_ = 0;
-  clear_has_pitch();
-}
-inline float RGBDImage::pitch() const {
-  return pitch_;
-}
-inline void RGBDImage::set_pitch(float value) {
-  set_has_pitch();
-  pitch_ = value;
-}
-
-// optional float yaw = 14;
-inline bool RGBDImage::has_yaw() const {
-  return (_has_bits_[0] & 0x00002000u) != 0;
-}
-inline void RGBDImage::set_has_yaw() {
-  _has_bits_[0] |= 0x00002000u;
-}
-inline void RGBDImage::clear_has_yaw() {
-  _has_bits_[0] &= ~0x00002000u;
-}
-inline void RGBDImage::clear_yaw() {
-  yaw_ = 0;
-  clear_has_yaw();
-}
-inline float RGBDImage::yaw() const {
-  return yaw_;
-}
-inline void RGBDImage::set_yaw(float value) {
-  set_has_yaw();
-  yaw_ = value;
-}
-
-// optional float lon = 15;
-inline bool RGBDImage::has_lon() const {
-  return (_has_bits_[0] & 0x00004000u) != 0;
-}
-inline void RGBDImage::set_has_lon() {
-  _has_bits_[0] |= 0x00004000u;
-}
-inline void RGBDImage::clear_has_lon() {
-  _has_bits_[0] &= ~0x00004000u;
-}
-inline void RGBDImage::clear_lon() {
-  lon_ = 0;
-  clear_has_lon();
-}
-inline float RGBDImage::lon() const {
-  return lon_;
-}
-inline void RGBDImage::set_lon(float value) {
-  set_has_lon();
-  lon_ = value;
-}
-
-// optional float lat = 16;
-inline bool RGBDImage::has_lat() const {
-  return (_has_bits_[0] & 0x00008000u) != 0;
-}
-inline void RGBDImage::set_has_lat() {
-  _has_bits_[0] |= 0x00008000u;
-}
-inline void RGBDImage::clear_has_lat() {
-  _has_bits_[0] &= ~0x00008000u;
-}
-inline void RGBDImage::clear_lat() {
-  lat_ = 0;
-  clear_has_lat();
-}
-inline float RGBDImage::lat() const {
-  return lat_;
-}
-inline void RGBDImage::set_lat(float value) {
-  set_has_lat();
-  lat_ = value;
-}
-
-// optional float alt = 17;
-inline bool RGBDImage::has_alt() const {
-  return (_has_bits_[0] & 0x00010000u) != 0;
-}
-inline void RGBDImage::set_has_alt() {
-  _has_bits_[0] |= 0x00010000u;
-}
-inline void RGBDImage::clear_has_alt() {
-  _has_bits_[0] &= ~0x00010000u;
-}
-inline void RGBDImage::clear_alt() {
-  alt_ = 0;
-  clear_has_alt();
-}
-inline float RGBDImage::alt() const {
-  return alt_;
-}
-inline void RGBDImage::set_alt(float value) {
-  set_has_alt();
-  alt_ = value;
-}
-
-// optional float ground_x = 18;
-inline bool RGBDImage::has_ground_x() const {
-  return (_has_bits_[0] & 0x00020000u) != 0;
-}
-inline void RGBDImage::set_has_ground_x() {
-  _has_bits_[0] |= 0x00020000u;
-}
-inline void RGBDImage::clear_has_ground_x() {
-  _has_bits_[0] &= ~0x00020000u;
-}
-inline void RGBDImage::clear_ground_x() {
-  ground_x_ = 0;
-  clear_has_ground_x();
-}
-inline float RGBDImage::ground_x() const {
-  return ground_x_;
-}
-inline void RGBDImage::set_ground_x(float value) {
-  set_has_ground_x();
-  ground_x_ = value;
-}
-
-// optional float ground_y = 19;
-inline bool RGBDImage::has_ground_y() const {
-  return (_has_bits_[0] & 0x00040000u) != 0;
-}
-inline void RGBDImage::set_has_ground_y() {
-  _has_bits_[0] |= 0x00040000u;
-}
-inline void RGBDImage::clear_has_ground_y() {
-  _has_bits_[0] &= ~0x00040000u;
-}
-inline void RGBDImage::clear_ground_y() {
-  ground_y_ = 0;
-  clear_has_ground_y();
-}
-inline float RGBDImage::ground_y() const {
-  return ground_y_;
-}
-inline void RGBDImage::set_ground_y(float value) {
-  set_has_ground_y();
-  ground_y_ = value;
-}
-
-// optional float ground_z = 20;
-inline bool RGBDImage::has_ground_z() const {
-  return (_has_bits_[0] & 0x00080000u) != 0;
-}
-inline void RGBDImage::set_has_ground_z() {
-  _has_bits_[0] |= 0x00080000u;
-}
-inline void RGBDImage::clear_has_ground_z() {
-  _has_bits_[0] &= ~0x00080000u;
-}
-inline void RGBDImage::clear_ground_z() {
-  ground_z_ = 0;
-  clear_has_ground_z();
-}
-inline float RGBDImage::ground_z() const {
-  return ground_z_;
-}
-inline void RGBDImage::set_ground_z(float value) {
-  set_has_ground_z();
-  ground_z_ = value;
-}
-
-// repeated float camera_matrix = 21;
-inline int RGBDImage::camera_matrix_size() const {
-  return camera_matrix_.size();
-}
-inline void RGBDImage::clear_camera_matrix() {
-  camera_matrix_.Clear();
-}
-inline float RGBDImage::camera_matrix(int index) const {
-  return camera_matrix_.Get(index);
-}
-inline void RGBDImage::set_camera_matrix(int index, float value) {
-  camera_matrix_.Set(index, value);
-}
-inline void RGBDImage::add_camera_matrix(float value) {
-  camera_matrix_.Add(value);
-}
-inline const ::google::protobuf::RepeatedField< float >&
-RGBDImage::camera_matrix() const {
-  return camera_matrix_;
-}
-inline ::google::protobuf::RepeatedField< float >*
-RGBDImage::mutable_camera_matrix() {
-  return &camera_matrix_;
-}
-
-// -------------------------------------------------------------------
-
-// Waypoint
-
-// required double x = 1;
-inline bool Waypoint::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void Waypoint::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void Waypoint::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void Waypoint::clear_x() {
-  x_ = 0;
-  clear_has_x();
-}
-inline double Waypoint::x() const {
-  return x_;
-}
-inline void Waypoint::set_x(double value) {
-  set_has_x();
-  x_ = value;
-}
-
-// required double y = 2;
-inline bool Waypoint::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void Waypoint::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void Waypoint::clear_has_y() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void Waypoint::clear_y() {
-  y_ = 0;
-  clear_has_y();
-}
-inline double Waypoint::y() const {
-  return y_;
-}
-inline void Waypoint::set_y(double value) {
-  set_has_y();
-  y_ = value;
-}
-
-// optional double z = 3;
-inline bool Waypoint::has_z() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void Waypoint::set_has_z() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void Waypoint::clear_has_z() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void Waypoint::clear_z() {
-  z_ = 0;
-  clear_has_z();
-}
-inline double Waypoint::z() const {
-  return z_;
-}
-inline void Waypoint::set_z(double value) {
-  set_has_z();
-  z_ = value;
-}
-
-// optional double roll = 4;
-inline bool Waypoint::has_roll() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void Waypoint::set_has_roll() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void Waypoint::clear_has_roll() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void Waypoint::clear_roll() {
-  roll_ = 0;
-  clear_has_roll();
-}
-inline double Waypoint::roll() const {
-  return roll_;
-}
-inline void Waypoint::set_roll(double value) {
-  set_has_roll();
-  roll_ = value;
-}
-
-// optional double pitch = 5;
-inline bool Waypoint::has_pitch() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void Waypoint::set_has_pitch() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void Waypoint::clear_has_pitch() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void Waypoint::clear_pitch() {
-  pitch_ = 0;
-  clear_has_pitch();
-}
-inline double Waypoint::pitch() const {
-  return pitch_;
-}
-inline void Waypoint::set_pitch(double value) {
-  set_has_pitch();
-  pitch_ = value;
-}
-
-// optional double yaw = 6;
-inline bool Waypoint::has_yaw() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void Waypoint::set_has_yaw() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void Waypoint::clear_has_yaw() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void Waypoint::clear_yaw() {
-  yaw_ = 0;
-  clear_has_yaw();
-}
-inline double Waypoint::yaw() const {
-  return yaw_;
-}
-inline void Waypoint::set_yaw(double value) {
-  set_has_yaw();
-  yaw_ = value;
-}
-
-
-// @@protoc_insertion_point(namespace_scope)
-
-}  // namespace px
-
-#ifndef SWIG
-namespace google {
-namespace protobuf {
-
-template <>
-inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_CoordinateFrameType>() {
-  return ::px::GLOverlay_CoordinateFrameType_descriptor();
-}
-template <>
-inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Mode>() {
-  return ::px::GLOverlay_Mode_descriptor();
-}
-template <>
-inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Identifier>() {
-  return ::px::GLOverlay_Identifier_descriptor();
-}
-
-}  // namespace google
-}  // namespace protobuf
-#endif  // SWIG
-
-// @@protoc_insertion_point(global_scope)
-
-#endif  // PROTOBUF_pixhawk_2eproto__INCLUDED
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h b/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h
deleted file mode 100644
index caa4d3dcb..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/testsuite.h
+++ /dev/null
@@ -1,996 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from pixhawk.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef PIXHAWK_TESTSUITE_H
-#define PIXHAWK_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg);
-static void mavlink_test_pixhawk(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_common(system_id, component_id, last_msg);
-	mavlink_test_pixhawk(system_id, component_id, last_msg);
-}
-#endif
-
-#include "../common/testsuite.h"
-
-
-static void mavlink_test_set_cam_shutter(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_set_cam_shutter_t packet_in = {
-		17.0,
-	}17443,
-	}17547,
-	}29,
-	}96,
-	}163,
-	};
-	mavlink_set_cam_shutter_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.gain = packet_in.gain;
-        	packet1.interval = packet_in.interval;
-        	packet1.exposure = packet_in.exposure;
-        	packet1.cam_no = packet_in.cam_no;
-        	packet1.cam_mode = packet_in.cam_mode;
-        	packet1.trigger_pin = packet_in.trigger_pin;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_cam_shutter_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_cam_shutter_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_cam_shutter_pack(system_id, component_id, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain );
-	mavlink_msg_set_cam_shutter_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain );
-	mavlink_msg_set_cam_shutter_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_set_cam_shutter_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_cam_shutter_send(MAVLINK_COMM_1 , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain );
-	mavlink_msg_set_cam_shutter_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_image_triggered(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_image_triggered_t packet_in = {
-		93372036854775807ULL,
-	}963497880,
-	}101.0,
-	}129.0,
-	}157.0,
-	}185.0,
-	}213.0,
-	}241.0,
-	}269.0,
-	}297.0,
-	}325.0,
-	}353.0,
-	};
-	mavlink_image_triggered_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.timestamp = packet_in.timestamp;
-        	packet1.seq = packet_in.seq;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.local_z = packet_in.local_z;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.ground_x = packet_in.ground_x;
-        	packet1.ground_y = packet_in.ground_y;
-        	packet1.ground_z = packet_in.ground_z;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_triggered_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_image_triggered_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_triggered_pack(system_id, component_id, &msg , packet1.timestamp , packet1.seq , packet1.roll , packet1.pitch , packet1.yaw , packet1.local_z , packet1.lat , packet1.lon , packet1.alt , packet1.ground_x , packet1.ground_y , packet1.ground_z );
-	mavlink_msg_image_triggered_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_triggered_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.timestamp , packet1.seq , packet1.roll , packet1.pitch , packet1.yaw , packet1.local_z , packet1.lat , packet1.lon , packet1.alt , packet1.ground_x , packet1.ground_y , packet1.ground_z );
-	mavlink_msg_image_triggered_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_image_triggered_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_triggered_send(MAVLINK_COMM_1 , packet1.timestamp , packet1.seq , packet1.roll , packet1.pitch , packet1.yaw , packet1.local_z , packet1.lat , packet1.lon , packet1.alt , packet1.ground_x , packet1.ground_y , packet1.ground_z );
-	mavlink_msg_image_triggered_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_image_trigger_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_image_trigger_control_t packet_in = {
-		5,
-	};
-	mavlink_image_trigger_control_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.enable = packet_in.enable;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_trigger_control_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_image_trigger_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_trigger_control_pack(system_id, component_id, &msg , packet1.enable );
-	mavlink_msg_image_trigger_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_trigger_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.enable );
-	mavlink_msg_image_trigger_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_image_trigger_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_trigger_control_send(MAVLINK_COMM_1 , packet1.enable );
-	mavlink_msg_image_trigger_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_image_available(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_image_available_t packet_in = {
-		93372036854775807ULL,
-	}93372036854776311ULL,
-	}93372036854776815ULL,
-	}963498712,
-	}963498920,
-	}963499128,
-	}963499336,
-	}297.0,
-	}325.0,
-	}353.0,
-	}381.0,
-	}409.0,
-	}437.0,
-	}465.0,
-	}493.0,
-	}521.0,
-	}549.0,
-	}577.0,
-	}21603,
-	}21707,
-	}21811,
-	}147,
-	}214,
-	};
-	mavlink_image_available_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.cam_id = packet_in.cam_id;
-        	packet1.timestamp = packet_in.timestamp;
-        	packet1.valid_until = packet_in.valid_until;
-        	packet1.img_seq = packet_in.img_seq;
-        	packet1.img_buf_index = packet_in.img_buf_index;
-        	packet1.key = packet_in.key;
-        	packet1.exposure = packet_in.exposure;
-        	packet1.gain = packet_in.gain;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.local_z = packet_in.local_z;
-        	packet1.lat = packet_in.lat;
-        	packet1.lon = packet_in.lon;
-        	packet1.alt = packet_in.alt;
-        	packet1.ground_x = packet_in.ground_x;
-        	packet1.ground_y = packet_in.ground_y;
-        	packet1.ground_z = packet_in.ground_z;
-        	packet1.width = packet_in.width;
-        	packet1.height = packet_in.height;
-        	packet1.depth = packet_in.depth;
-        	packet1.cam_no = packet_in.cam_no;
-        	packet1.channels = packet_in.channels;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_available_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_image_available_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_available_pack(system_id, component_id, &msg , packet1.cam_id , packet1.cam_no , packet1.timestamp , packet1.valid_until , packet1.img_seq , packet1.img_buf_index , packet1.width , packet1.height , packet1.depth , packet1.channels , packet1.key , packet1.exposure , packet1.gain , packet1.roll , packet1.pitch , packet1.yaw , packet1.local_z , packet1.lat , packet1.lon , packet1.alt , packet1.ground_x , packet1.ground_y , packet1.ground_z );
-	mavlink_msg_image_available_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_available_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.cam_id , packet1.cam_no , packet1.timestamp , packet1.valid_until , packet1.img_seq , packet1.img_buf_index , packet1.width , packet1.height , packet1.depth , packet1.channels , packet1.key , packet1.exposure , packet1.gain , packet1.roll , packet1.pitch , packet1.yaw , packet1.local_z , packet1.lat , packet1.lon , packet1.alt , packet1.ground_x , packet1.ground_y , packet1.ground_z );
-	mavlink_msg_image_available_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_image_available_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_image_available_send(MAVLINK_COMM_1 , packet1.cam_id , packet1.cam_no , packet1.timestamp , packet1.valid_until , packet1.img_seq , packet1.img_buf_index , packet1.width , packet1.height , packet1.depth , packet1.channels , packet1.key , packet1.exposure , packet1.gain , packet1.roll , packet1.pitch , packet1.yaw , packet1.local_z , packet1.lat , packet1.lon , packet1.alt , packet1.ground_x , packet1.ground_y , packet1.ground_z );
-	mavlink_msg_image_available_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_position_control_offset(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_set_position_control_offset_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}53,
-	}120,
-	};
-	mavlink_set_position_control_offset_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_position_control_offset_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_position_control_offset_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_position_control_offset_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_set_position_control_offset_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_position_control_offset_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_set_position_control_offset_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_set_position_control_offset_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_position_control_offset_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_set_position_control_offset_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_position_control_setpoint(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_position_control_setpoint_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}18067,
-	};
-	mavlink_position_control_setpoint_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.id = packet_in.id;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_position_control_setpoint_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_position_control_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_position_control_setpoint_pack(system_id, component_id, &msg , packet1.id , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_position_control_setpoint_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_position_control_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_position_control_setpoint_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_position_control_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_1 , packet1.id , packet1.x , packet1.y , packet1.z , packet1.yaw );
-	mavlink_msg_position_control_setpoint_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_marker(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_marker_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}18483,
-	};
-	mavlink_marker_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.id = packet_in.id;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_marker_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_marker_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_marker_pack(system_id, component_id, &msg , packet1.id , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_marker_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_marker_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_marker_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_marker_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_marker_send(MAVLINK_COMM_1 , packet1.id , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
-	mavlink_msg_marker_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_raw_aux(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_raw_aux_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}17755,
-	}17859,
-	}17963,
-	};
-	mavlink_raw_aux_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.baro = packet_in.baro;
-        	packet1.adc1 = packet_in.adc1;
-        	packet1.adc2 = packet_in.adc2;
-        	packet1.adc3 = packet_in.adc3;
-        	packet1.adc4 = packet_in.adc4;
-        	packet1.vbat = packet_in.vbat;
-        	packet1.temp = packet_in.temp;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_aux_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_raw_aux_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_aux_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.vbat , packet1.temp , packet1.baro );
-	mavlink_msg_raw_aux_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_aux_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.vbat , packet1.temp , packet1.baro );
-	mavlink_msg_raw_aux_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_raw_aux_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_raw_aux_send(MAVLINK_COMM_1 , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.vbat , packet1.temp , packet1.baro );
-	mavlink_msg_raw_aux_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_watchdog_heartbeat(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_watchdog_heartbeat_t packet_in = {
-		17235,
-	}17339,
-	};
-	mavlink_watchdog_heartbeat_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.watchdog_id = packet_in.watchdog_id;
-        	packet1.process_count = packet_in.process_count;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_heartbeat_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_watchdog_heartbeat_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_heartbeat_pack(system_id, component_id, &msg , packet1.watchdog_id , packet1.process_count );
-	mavlink_msg_watchdog_heartbeat_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.watchdog_id , packet1.process_count );
-	mavlink_msg_watchdog_heartbeat_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_watchdog_heartbeat_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_heartbeat_send(MAVLINK_COMM_1 , packet1.watchdog_id , packet1.process_count );
-	mavlink_msg_watchdog_heartbeat_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_watchdog_process_info(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_watchdog_process_info_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABC",
-	}"EFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRST",
-	};
-	mavlink_watchdog_process_info_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.timeout = packet_in.timeout;
-        	packet1.watchdog_id = packet_in.watchdog_id;
-        	packet1.process_id = packet_in.process_id;
-        
-        	mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*100);
-        	mav_array_memcpy(packet1.arguments, packet_in.arguments, sizeof(char)*147);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_process_info_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_watchdog_process_info_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_process_info_pack(system_id, component_id, &msg , packet1.watchdog_id , packet1.process_id , packet1.name , packet1.arguments , packet1.timeout );
-	mavlink_msg_watchdog_process_info_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_process_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.watchdog_id , packet1.process_id , packet1.name , packet1.arguments , packet1.timeout );
-	mavlink_msg_watchdog_process_info_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_watchdog_process_info_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_process_info_send(MAVLINK_COMM_1 , packet1.watchdog_id , packet1.process_id , packet1.name , packet1.arguments , packet1.timeout );
-	mavlink_msg_watchdog_process_info_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_watchdog_process_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_watchdog_process_status_t packet_in = {
-		963497464,
-	}17443,
-	}17547,
-	}17651,
-	}163,
-	}230,
-	};
-	mavlink_watchdog_process_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.pid = packet_in.pid;
-        	packet1.watchdog_id = packet_in.watchdog_id;
-        	packet1.process_id = packet_in.process_id;
-        	packet1.crashes = packet_in.crashes;
-        	packet1.state = packet_in.state;
-        	packet1.muted = packet_in.muted;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_process_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_watchdog_process_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_process_status_pack(system_id, component_id, &msg , packet1.watchdog_id , packet1.process_id , packet1.state , packet1.muted , packet1.pid , packet1.crashes );
-	mavlink_msg_watchdog_process_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_process_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.watchdog_id , packet1.process_id , packet1.state , packet1.muted , packet1.pid , packet1.crashes );
-	mavlink_msg_watchdog_process_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_watchdog_process_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_process_status_send(MAVLINK_COMM_1 , packet1.watchdog_id , packet1.process_id , packet1.state , packet1.muted , packet1.pid , packet1.crashes );
-	mavlink_msg_watchdog_process_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_watchdog_command(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_watchdog_command_t packet_in = {
-		17235,
-	}17339,
-	}17,
-	}84,
-	};
-	mavlink_watchdog_command_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.watchdog_id = packet_in.watchdog_id;
-        	packet1.process_id = packet_in.process_id;
-        	packet1.target_system_id = packet_in.target_system_id;
-        	packet1.command_id = packet_in.command_id;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_command_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_watchdog_command_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_command_pack(system_id, component_id, &msg , packet1.target_system_id , packet1.watchdog_id , packet1.process_id , packet1.command_id );
-	mavlink_msg_watchdog_command_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_command_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system_id , packet1.watchdog_id , packet1.process_id , packet1.command_id );
-	mavlink_msg_watchdog_command_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_watchdog_command_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_watchdog_command_send(MAVLINK_COMM_1 , packet1.target_system_id , packet1.watchdog_id , packet1.process_id , packet1.command_id );
-	mavlink_msg_watchdog_command_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_pattern_detected(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_pattern_detected_t packet_in = {
-		17.0,
-	}17,
-	}"FGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ",
-	}128,
-	};
-	mavlink_pattern_detected_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.confidence = packet_in.confidence;
-        	packet1.type = packet_in.type;
-        	packet1.detected = packet_in.detected;
-        
-        	mav_array_memcpy(packet1.file, packet_in.file, sizeof(char)*100);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_pattern_detected_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_pattern_detected_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_pattern_detected_pack(system_id, component_id, &msg , packet1.type , packet1.confidence , packet1.file , packet1.detected );
-	mavlink_msg_pattern_detected_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_pattern_detected_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.confidence , packet1.file , packet1.detected );
-	mavlink_msg_pattern_detected_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_pattern_detected_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_pattern_detected_send(MAVLINK_COMM_1 , packet1.type , packet1.confidence , packet1.file , packet1.detected );
-	mavlink_msg_pattern_detected_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_point_of_interest(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_point_of_interest_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}17859,
-	}175,
-	}242,
-	}53,
-	}"RSTUVWXYZABCDEFGHIJKLMNOP",
-	};
-	mavlink_point_of_interest_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.timeout = packet_in.timeout;
-        	packet1.type = packet_in.type;
-        	packet1.color = packet_in.color;
-        	packet1.coordinate_system = packet_in.coordinate_system;
-        
-        	mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*26);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_point_of_interest_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_point_of_interest_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_point_of_interest_pack(system_id, component_id, &msg , packet1.type , packet1.color , packet1.coordinate_system , packet1.timeout , packet1.x , packet1.y , packet1.z , packet1.name );
-	mavlink_msg_point_of_interest_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_point_of_interest_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.color , packet1.coordinate_system , packet1.timeout , packet1.x , packet1.y , packet1.z , packet1.name );
-	mavlink_msg_point_of_interest_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_point_of_interest_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_point_of_interest_send(MAVLINK_COMM_1 , packet1.type , packet1.color , packet1.coordinate_system , packet1.timeout , packet1.x , packet1.y , packet1.z , packet1.name );
-	mavlink_msg_point_of_interest_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_point_of_interest_connection(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_point_of_interest_connection_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}129.0,
-	}157.0,
-	}18483,
-	}211,
-	}22,
-	}89,
-	}"DEFGHIJKLMNOPQRSTUVWXYZAB",
-	};
-	mavlink_point_of_interest_connection_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.xp1 = packet_in.xp1;
-        	packet1.yp1 = packet_in.yp1;
-        	packet1.zp1 = packet_in.zp1;
-        	packet1.xp2 = packet_in.xp2;
-        	packet1.yp2 = packet_in.yp2;
-        	packet1.zp2 = packet_in.zp2;
-        	packet1.timeout = packet_in.timeout;
-        	packet1.type = packet_in.type;
-        	packet1.color = packet_in.color;
-        	packet1.coordinate_system = packet_in.coordinate_system;
-        
-        	mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*26);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_point_of_interest_connection_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_point_of_interest_connection_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_point_of_interest_connection_pack(system_id, component_id, &msg , packet1.type , packet1.color , packet1.coordinate_system , packet1.timeout , packet1.xp1 , packet1.yp1 , packet1.zp1 , packet1.xp2 , packet1.yp2 , packet1.zp2 , packet1.name );
-	mavlink_msg_point_of_interest_connection_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_point_of_interest_connection_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.color , packet1.coordinate_system , packet1.timeout , packet1.xp1 , packet1.yp1 , packet1.zp1 , packet1.xp2 , packet1.yp2 , packet1.zp2 , packet1.name );
-	mavlink_msg_point_of_interest_connection_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_point_of_interest_connection_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_point_of_interest_connection_send(MAVLINK_COMM_1 , packet1.type , packet1.color , packet1.coordinate_system , packet1.timeout , packet1.xp1 , packet1.yp1 , packet1.zp1 , packet1.xp2 , packet1.yp2 , packet1.zp2 , packet1.name );
-	mavlink_msg_point_of_interest_connection_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_brief_feature(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_brief_feature_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}18067,
-	}18171,
-	}65,
-	}{ 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 },
-	};
-	mavlink_brief_feature_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.x = packet_in.x;
-        	packet1.y = packet_in.y;
-        	packet1.z = packet_in.z;
-        	packet1.response = packet_in.response;
-        	packet1.size = packet_in.size;
-        	packet1.orientation = packet_in.orientation;
-        	packet1.orientation_assignment = packet_in.orientation_assignment;
-        
-        	mav_array_memcpy(packet1.descriptor, packet_in.descriptor, sizeof(uint8_t)*32);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_brief_feature_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_brief_feature_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_brief_feature_pack(system_id, component_id, &msg , packet1.x , packet1.y , packet1.z , packet1.orientation_assignment , packet1.size , packet1.orientation , packet1.descriptor , packet1.response );
-	mavlink_msg_brief_feature_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_brief_feature_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.x , packet1.y , packet1.z , packet1.orientation_assignment , packet1.size , packet1.orientation , packet1.descriptor , packet1.response );
-	mavlink_msg_brief_feature_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_brief_feature_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_brief_feature_send(MAVLINK_COMM_1 , packet1.x , packet1.y , packet1.z , packet1.orientation_assignment , packet1.size , packet1.orientation , packet1.descriptor , packet1.response );
-	mavlink_msg_brief_feature_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_attitude_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_attitude_control_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	}101.0,
-	}53,
-	}120,
-	}187,
-	}254,
-	}65,
-	};
-	mavlink_attitude_control_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.thrust = packet_in.thrust;
-        	packet1.target = packet_in.target;
-        	packet1.roll_manual = packet_in.roll_manual;
-        	packet1.pitch_manual = packet_in.pitch_manual;
-        	packet1.yaw_manual = packet_in.yaw_manual;
-        	packet1.thrust_manual = packet_in.thrust_manual;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_control_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_attitude_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_control_pack(system_id, component_id, &msg , packet1.target , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.roll_manual , packet1.pitch_manual , packet1.yaw_manual , packet1.thrust_manual );
-	mavlink_msg_attitude_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.roll_manual , packet1.pitch_manual , packet1.yaw_manual , packet1.thrust_manual );
-	mavlink_msg_attitude_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_attitude_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_attitude_control_send(MAVLINK_COMM_1 , packet1.target , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.roll_manual , packet1.pitch_manual , packet1.yaw_manual , packet1.thrust_manual );
-	mavlink_msg_attitude_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_pixhawk(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_set_cam_shutter(system_id, component_id, last_msg);
-	mavlink_test_image_triggered(system_id, component_id, last_msg);
-	mavlink_test_image_trigger_control(system_id, component_id, last_msg);
-	mavlink_test_image_available(system_id, component_id, last_msg);
-	mavlink_test_set_position_control_offset(system_id, component_id, last_msg);
-	mavlink_test_position_control_setpoint(system_id, component_id, last_msg);
-	mavlink_test_marker(system_id, component_id, last_msg);
-	mavlink_test_raw_aux(system_id, component_id, last_msg);
-	mavlink_test_watchdog_heartbeat(system_id, component_id, last_msg);
-	mavlink_test_watchdog_process_info(system_id, component_id, last_msg);
-	mavlink_test_watchdog_process_status(system_id, component_id, last_msg);
-	mavlink_test_watchdog_command(system_id, component_id, last_msg);
-	mavlink_test_pattern_detected(system_id, component_id, last_msg);
-	mavlink_test_point_of_interest(system_id, component_id, last_msg);
-	mavlink_test_point_of_interest_connection(system_id, component_id, last_msg);
-	mavlink_test_brief_feature(system_id, component_id, last_msg);
-	mavlink_test_attitude_control(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // PIXHAWK_TESTSUITE_H
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h
deleted file mode 100644
index aa79f6ffb..000000000
--- a/mavlink/include/mavlink/v1.0/pixhawk/version.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from pixhawk.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:16 2014"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
- 
-#endif // MAVLINK_VERSION_H
diff --git a/mavlink/include/mavlink/v1.0/protocol.h b/mavlink/include/mavlink/v1.0/protocol.h
deleted file mode 100644
index 86e7ff588..000000000
--- a/mavlink/include/mavlink/v1.0/protocol.h
+++ /dev/null
@@ -1,328 +0,0 @@
-#ifndef  _MAVLINK_PROTOCOL_H_
-#define  _MAVLINK_PROTOCOL_H_
-
-#include "string.h"
-#include "mavlink_types.h"
-
-/* 
-   If you want MAVLink on a system that is native big-endian,
-   you need to define NATIVE_BIG_ENDIAN
-*/
-#ifdef NATIVE_BIG_ENDIAN
-# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
-#else
-# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
-#endif
-
-#ifndef MAVLINK_STACK_BUFFER
-#define MAVLINK_STACK_BUFFER 0
-#endif
-
-#ifndef MAVLINK_AVOID_GCC_STACK_BUG
-# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
-#endif
-
-#ifndef MAVLINK_ASSERT
-#define MAVLINK_ASSERT(x)
-#endif
-
-#ifndef MAVLINK_START_UART_SEND
-#define MAVLINK_START_UART_SEND(chan, length)
-#endif
-
-#ifndef MAVLINK_END_UART_SEND
-#define MAVLINK_END_UART_SEND(chan, length)
-#endif
-
-#ifdef MAVLINK_SEPARATE_HELPERS
-#define MAVLINK_HELPER
-#else
-#define MAVLINK_HELPER static inline
-#include "mavlink_helpers.h"
-#endif // MAVLINK_SEPARATE_HELPERS
-
-/* always include the prototypes to ensure we don't get out of sync */
-#ifndef MAVLINK_GET_CHANNEL_STATUS
-MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
-#endif
-MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length, uint8_t crc_extra);
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length, uint8_t crc_extra);
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
-						    uint8_t length, uint8_t crc_extra);
-#endif
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length);
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length);
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
-#endif
-#endif // MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
-MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
-MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
-MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
-MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, 
-					       uint8_t* r_bit_index, uint8_t* buffer);
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
-MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg);
-#endif
-
-/**
- * @brief Get the required buffer size for this message
- */
-static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
-{
-	return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-static inline void byte_swap_2(char *dst, const char *src)
-{
-	dst[0] = src[1];
-	dst[1] = src[0];
-}
-static inline void byte_swap_4(char *dst, const char *src)
-{
-	dst[0] = src[3];
-	dst[1] = src[2];
-	dst[2] = src[1];
-	dst[3] = src[0];
-}
-static inline void byte_swap_8(char *dst, const char *src)
-{
-	dst[0] = src[7];
-	dst[1] = src[6];
-	dst[2] = src[5];
-	dst[3] = src[4];
-	dst[4] = src[3];
-	dst[5] = src[2];
-	dst[6] = src[1];
-	dst[7] = src[0];
-}
-#elif !MAVLINK_ALIGNED_FIELDS
-static inline void byte_copy_2(char *dst, const char *src)
-{
-	dst[0] = src[0];
-	dst[1] = src[1];
-}
-static inline void byte_copy_4(char *dst, const char *src)
-{
-	dst[0] = src[0];
-	dst[1] = src[1];
-	dst[2] = src[2];
-	dst[3] = src[3];
-}
-static inline void byte_copy_8(char *dst, const char *src)
-{
-	memcpy(dst, src, 8);
-}
-#endif
-
-#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
-#define _mav_put_int8_t(buf, wire_offset, b)  buf[wire_offset] = (int8_t)b
-#define _mav_put_char(buf, wire_offset, b)    buf[wire_offset] = b
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int16_t(buf, wire_offset, b)  byte_swap_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int32_t(buf, wire_offset, b)  byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int64_t(buf, wire_offset, b)  byte_swap_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_float(buf, wire_offset, b)    byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_double(buf, wire_offset, b)   byte_swap_8(&buf[wire_offset], (const char *)&b)
-#elif !MAVLINK_ALIGNED_FIELDS
-#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int16_t(buf, wire_offset, b)  byte_copy_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int32_t(buf, wire_offset, b)  byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int64_t(buf, wire_offset, b)  byte_copy_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_float(buf, wire_offset, b)    byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_double(buf, wire_offset, b)   byte_copy_8(&buf[wire_offset], (const char *)&b)
-#else
-#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
-#define _mav_put_int16_t(buf, wire_offset, b)  *(int16_t *)&buf[wire_offset] = b
-#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
-#define _mav_put_int32_t(buf, wire_offset, b)  *(int32_t *)&buf[wire_offset] = b
-#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
-#define _mav_put_int64_t(buf, wire_offset, b)  *(int64_t *)&buf[wire_offset] = b
-#define _mav_put_float(buf, wire_offset, b)    *(float *)&buf[wire_offset] = b
-#define _mav_put_double(buf, wire_offset, b)   *(double *)&buf[wire_offset] = b
-#endif
-
-/*
-  like memcpy(), but if src is NULL, do a memset to zero
-*/
-static inline void mav_array_memcpy(void *dest, const void *src, size_t n)
-{
-	if (src == NULL) {
-		memset(dest, 0, n);
-	} else {
-		memcpy(dest, src, n);
-	}
-}
-
-/*
- * Place a char array into a buffer
- */
-static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-
-}
-
-/*
- * Place a uint8_t array into a buffer
- */
-static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-
-}
-
-/*
- * Place a int8_t array into a buffer
- */
-static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_PUT_ARRAY(TYPE, V) \
-static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
-{ \
-	if (b == NULL) { \
-		memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
-	} else { \
-		uint16_t i; \
-		for (i=0; i<array_length; i++) { \
-			_mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
-		} \
-	} \
-}
-#else
-#define _MAV_PUT_ARRAY(TYPE, V)					\
-static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
-{ \
-	mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
-}
-#endif
-
-_MAV_PUT_ARRAY(uint16_t, u16)
-_MAV_PUT_ARRAY(uint32_t, u32)
-_MAV_PUT_ARRAY(uint64_t, u64)
-_MAV_PUT_ARRAY(int16_t,  i16)
-_MAV_PUT_ARRAY(int32_t,  i32)
-_MAV_PUT_ARRAY(int64_t,  i64)
-_MAV_PUT_ARRAY(float,    f)
-_MAV_PUT_ARRAY(double,   d)
-
-#define _MAV_RETURN_char(msg, wire_offset)             (const char)_MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_int8_t(msg, wire_offset)   (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
-
-_MAV_MSG_RETURN_TYPE(uint16_t, 2)
-_MAV_MSG_RETURN_TYPE(int16_t,  2)
-_MAV_MSG_RETURN_TYPE(uint32_t, 4)
-_MAV_MSG_RETURN_TYPE(int32_t,  4)
-_MAV_MSG_RETURN_TYPE(uint64_t, 8)
-_MAV_MSG_RETURN_TYPE(int64_t,  8)
-_MAV_MSG_RETURN_TYPE(float,    4)
-_MAV_MSG_RETURN_TYPE(double,   8)
-
-#elif !MAVLINK_ALIGNED_FIELDS
-#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
-
-_MAV_MSG_RETURN_TYPE(uint16_t, 2)
-_MAV_MSG_RETURN_TYPE(int16_t,  2)
-_MAV_MSG_RETURN_TYPE(uint32_t, 4)
-_MAV_MSG_RETURN_TYPE(int32_t,  4)
-_MAV_MSG_RETURN_TYPE(uint64_t, 8)
-_MAV_MSG_RETURN_TYPE(int64_t,  8)
-_MAV_MSG_RETURN_TYPE(float,    4)
-_MAV_MSG_RETURN_TYPE(double,   8)
-#else // nicely aligned, no swap
-#define _MAV_MSG_RETURN_TYPE(TYPE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
-
-_MAV_MSG_RETURN_TYPE(uint16_t)
-_MAV_MSG_RETURN_TYPE(int16_t)
-_MAV_MSG_RETURN_TYPE(uint32_t)
-_MAV_MSG_RETURN_TYPE(int32_t)
-_MAV_MSG_RETURN_TYPE(uint64_t)
-_MAV_MSG_RETURN_TYPE(int64_t)
-_MAV_MSG_RETURN_TYPE(float)
-_MAV_MSG_RETURN_TYPE(double)
-#endif // MAVLINK_NEED_BYTE_SWAP
-
-static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, 
-						     uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, 
-							uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, 
-						       uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_RETURN_ARRAY(TYPE, V) \
-static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
-							 uint8_t array_length, uint8_t wire_offset) \
-{ \
-	uint16_t i; \
-	for (i=0; i<array_length; i++) { \
-		value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
-	} \
-	return array_length*sizeof(value[0]); \
-}
-#else
-#define _MAV_RETURN_ARRAY(TYPE, V)					\
-static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
-							 uint8_t array_length, uint8_t wire_offset) \
-{ \
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
-	return array_length*sizeof(TYPE); \
-}
-#endif
-
-_MAV_RETURN_ARRAY(uint16_t, u16)
-_MAV_RETURN_ARRAY(uint32_t, u32)
-_MAV_RETURN_ARRAY(uint64_t, u64)
-_MAV_RETURN_ARRAY(int16_t,  i16)
-_MAV_RETURN_ARRAY(int32_t,  i32)
-_MAV_RETURN_ARRAY(int64_t,  i64)
-_MAV_RETURN_ARRAY(float,    f)
-_MAV_RETURN_ARRAY(double,   d)
-
-#endif // _MAVLINK_PROTOCOL_H_
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink.h
deleted file mode 100644
index c0a0a8967..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from sensesoar.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "sensesoar.h"
-
-#endif // MAVLINK_H
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
deleted file mode 100644
index 73e9d75db..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h
+++ /dev/null
@@ -1,219 +0,0 @@
-// MESSAGE CMD_AIRSPEED_ACK PACKING
-
-#define MAVLINK_MSG_ID_CMD_AIRSPEED_ACK 194
-
-typedef struct __mavlink_cmd_airspeed_ack_t
-{
- float spCmd; ///< 
-                
-            
- uint8_t ack; ///< 
-                
-            
-} mavlink_cmd_airspeed_ack_t;
-
-#define MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN 5
-#define MAVLINK_MSG_ID_194_LEN 5
-
-#define MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC 243
-#define MAVLINK_MSG_ID_194_CRC 243
-
-
-
-#define MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK { \
-	"CMD_AIRSPEED_ACK", \
-	2, \
-	{  { "spCmd", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_cmd_airspeed_ack_t, spCmd) }, \
-         { "ack", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_cmd_airspeed_ack_t, ack) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a cmd_airspeed_ack 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 spCmd 
-                
-            
- * @param ack 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float spCmd, uint8_t ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN];
-	_mav_put_float(buf, 0, spCmd);
-	_mav_put_uint8_t(buf, 4, ack);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
-#else
-	mavlink_cmd_airspeed_ack_t packet;
-	packet.spCmd = spCmd;
-	packet.ack = ack;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Pack a cmd_airspeed_ack 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 spCmd 
-                
-            
- * @param ack 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float spCmd,uint8_t ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN];
-	_mav_put_float(buf, 0, spCmd);
-	_mav_put_uint8_t(buf, 4, ack);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
-#else
-	mavlink_cmd_airspeed_ack_t packet;
-	packet.spCmd = spCmd;
-	packet.ack = ack;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
-#endif
-}
-
-/**
- * @brief Encode a cmd_airspeed_ack 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 cmd_airspeed_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack)
-{
-	return mavlink_msg_cmd_airspeed_ack_pack(system_id, component_id, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack);
-}
-
-/**
- * @brief Encode a cmd_airspeed_ack 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 cmd_airspeed_ack C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack)
-{
-	return mavlink_msg_cmd_airspeed_ack_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack);
-}
-
-/**
- * @brief Send a cmd_airspeed_ack message
- * @param chan MAVLink channel to send the message
- *
- * @param spCmd 
-                
-            
- * @param ack 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_cmd_airspeed_ack_send(mavlink_channel_t chan, float spCmd, uint8_t ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN];
-	_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;
-	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
-
-// MESSAGE CMD_AIRSPEED_ACK UNPACKING
-
-
-/**
- * @brief Get field spCmd from cmd_airspeed_ack message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_cmd_airspeed_ack_get_spCmd(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field ack from cmd_airspeed_ack message
- *
- * @return 
-                
-            
- */
-static inline uint8_t mavlink_msg_cmd_airspeed_ack_get_ack(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Decode a cmd_airspeed_ack message into a struct
- *
- * @param msg The message to decode
- * @param cmd_airspeed_ack C-struct to decode the message contents into
- */
-static inline void mavlink_msg_cmd_airspeed_ack_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	cmd_airspeed_ack->spCmd = mavlink_msg_cmd_airspeed_ack_get_spCmd(msg);
-	cmd_airspeed_ack->ack = mavlink_msg_cmd_airspeed_ack_get_ack(msg);
-#else
-	memcpy(cmd_airspeed_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
-#endif
-}
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
deleted file mode 100644
index b6b567f99..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h
+++ /dev/null
@@ -1,219 +0,0 @@
-// MESSAGE CMD_AIRSPEED_CHNG PACKING
-
-#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG 192
-
-typedef struct __mavlink_cmd_airspeed_chng_t
-{
- float spCmd; ///< 
-                
-            
- uint8_t target; ///< 
-                
-            
-} mavlink_cmd_airspeed_chng_t;
-
-#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN 5
-#define MAVLINK_MSG_ID_192_LEN 5
-
-#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC 209
-#define MAVLINK_MSG_ID_192_CRC 209
-
-
-
-#define MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG { \
-	"CMD_AIRSPEED_CHNG", \
-	2, \
-	{  { "spCmd", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_cmd_airspeed_chng_t, spCmd) }, \
-         { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_cmd_airspeed_chng_t, target) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a cmd_airspeed_chng 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 target 
-                
-            
- * @param spCmd 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target, float spCmd)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN];
-	_mav_put_float(buf, 0, spCmd);
-	_mav_put_uint8_t(buf, 4, target);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
-#else
-	mavlink_cmd_airspeed_chng_t packet;
-	packet.spCmd = spCmd;
-	packet.target = target;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
-#endif
-}
-
-/**
- * @brief Pack a cmd_airspeed_chng 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 target 
-                
-            
- * @param spCmd 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target,float spCmd)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN];
-	_mav_put_float(buf, 0, spCmd);
-	_mav_put_uint8_t(buf, 4, target);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
-#else
-	mavlink_cmd_airspeed_chng_t packet;
-	packet.spCmd = spCmd;
-	packet.target = target;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
-#endif
-}
-
-/**
- * @brief Encode a cmd_airspeed_chng 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 cmd_airspeed_chng C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng)
-{
-	return mavlink_msg_cmd_airspeed_chng_pack(system_id, component_id, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd);
-}
-
-/**
- * @brief Encode a cmd_airspeed_chng 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 cmd_airspeed_chng C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng)
-{
-	return mavlink_msg_cmd_airspeed_chng_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd);
-}
-
-/**
- * @brief Send a cmd_airspeed_chng message
- * @param chan MAVLink channel to send the message
- *
- * @param target 
-                
-            
- * @param spCmd 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_cmd_airspeed_chng_send(mavlink_channel_t chan, uint8_t target, float spCmd)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN];
-	_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;
-	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
-
-// MESSAGE CMD_AIRSPEED_CHNG UNPACKING
-
-
-/**
- * @brief Get field target from cmd_airspeed_chng message
- *
- * @return 
-                
-            
- */
-static inline uint8_t mavlink_msg_cmd_airspeed_chng_get_target(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field spCmd from cmd_airspeed_chng message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_cmd_airspeed_chng_get_spCmd(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Decode a cmd_airspeed_chng message into a struct
- *
- * @param msg The message to decode
- * @param cmd_airspeed_chng C-struct to decode the message contents into
- */
-static inline void mavlink_msg_cmd_airspeed_chng_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	cmd_airspeed_chng->spCmd = mavlink_msg_cmd_airspeed_chng_get_spCmd(msg);
-	cmd_airspeed_chng->target = mavlink_msg_cmd_airspeed_chng_get_target(msg);
-#else
-	memcpy(cmd_airspeed_chng, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
-#endif
-}
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
deleted file mode 100644
index 642f7aab9..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h
+++ /dev/null
@@ -1,187 +0,0 @@
-// MESSAGE FILT_ROT_VEL PACKING
-
-#define MAVLINK_MSG_ID_FILT_ROT_VEL 184
-
-typedef struct __mavlink_filt_rot_vel_t
-{
- float rotVel[3]; ///< 
-                
-            
-} mavlink_filt_rot_vel_t;
-
-#define MAVLINK_MSG_ID_FILT_ROT_VEL_LEN 12
-#define MAVLINK_MSG_ID_184_LEN 12
-
-#define MAVLINK_MSG_ID_FILT_ROT_VEL_CRC 79
-#define MAVLINK_MSG_ID_184_CRC 79
-
-#define MAVLINK_MSG_FILT_ROT_VEL_FIELD_ROTVEL_LEN 3
-
-#define MAVLINK_MESSAGE_INFO_FILT_ROT_VEL { \
-	"FILT_ROT_VEL", \
-	1, \
-	{  { "rotVel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_filt_rot_vel_t, rotVel) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a filt_rot_vel 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 rotVel 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const float *rotVel)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN];
-
-	_mav_put_float_array(buf, 0, rotVel, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
-#else
-	mavlink_filt_rot_vel_t packet;
-
-	mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a filt_rot_vel 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 rotVel 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const float *rotVel)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN];
-
-	_mav_put_float_array(buf, 0, rotVel, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
-#else
-	mavlink_filt_rot_vel_t packet;
-
-	mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a filt_rot_vel 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 filt_rot_vel C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel)
-{
-	return mavlink_msg_filt_rot_vel_pack(system_id, component_id, msg, filt_rot_vel->rotVel);
-}
-
-/**
- * @brief Encode a filt_rot_vel 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 filt_rot_vel C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_filt_rot_vel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel)
-{
-	return mavlink_msg_filt_rot_vel_pack_chan(system_id, component_id, chan, msg, filt_rot_vel->rotVel);
-}
-
-/**
- * @brief Send a filt_rot_vel message
- * @param chan MAVLink channel to send the message
- *
- * @param rotVel 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_filt_rot_vel_send(mavlink_channel_t chan, const float *rotVel)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FILT_ROT_VEL_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE FILT_ROT_VEL UNPACKING
-
-
-/**
- * @brief Get field rotVel from filt_rot_vel message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_filt_rot_vel_get_rotVel(const mavlink_message_t* msg, float *rotVel)
-{
-	return _MAV_RETURN_float_array(msg, rotVel, 3,  0);
-}
-
-/**
- * @brief Decode a filt_rot_vel message into a struct
- *
- * @param msg The message to decode
- * @param filt_rot_vel C-struct to decode the message contents into
- */
-static inline void mavlink_msg_filt_rot_vel_decode(const mavlink_message_t* msg, mavlink_filt_rot_vel_t* filt_rot_vel)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_filt_rot_vel_get_rotVel(msg, filt_rot_vel->rotVel);
-#else
-	memcpy(filt_rot_vel, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
-#endif
-}
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
deleted file mode 100644
index 0f8369881..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h
+++ /dev/null
@@ -1,220 +0,0 @@
-// MESSAGE LLC_OUT PACKING
-
-#define MAVLINK_MSG_ID_LLC_OUT 186
-
-typedef struct __mavlink_llc_out_t
-{
- int16_t servoOut[4]; ///< 
-                
-            
- int16_t MotorOut[2]; ///< 
-                
-            
-} mavlink_llc_out_t;
-
-#define MAVLINK_MSG_ID_LLC_OUT_LEN 12
-#define MAVLINK_MSG_ID_186_LEN 12
-
-#define MAVLINK_MSG_ID_LLC_OUT_CRC 5
-#define MAVLINK_MSG_ID_186_CRC 5
-
-#define MAVLINK_MSG_LLC_OUT_FIELD_SERVOOUT_LEN 4
-#define MAVLINK_MSG_LLC_OUT_FIELD_MOTOROUT_LEN 2
-
-#define MAVLINK_MESSAGE_INFO_LLC_OUT { \
-	"LLC_OUT", \
-	2, \
-	{  { "servoOut", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_llc_out_t, servoOut) }, \
-         { "MotorOut", NULL, MAVLINK_TYPE_INT16_T, 2, 8, offsetof(mavlink_llc_out_t, MotorOut) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a llc_out 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 servoOut 
-                
-            
- * @param MotorOut 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const int16_t *servoOut, const int16_t *MotorOut)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LLC_OUT_LEN];
-
-	_mav_put_int16_t_array(buf, 0, servoOut, 4);
-	_mav_put_int16_t_array(buf, 8, MotorOut, 2);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LLC_OUT_LEN);
-#else
-	mavlink_llc_out_t packet;
-
-	mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4);
-	mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LLC_OUT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LLC_OUT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LLC_OUT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a llc_out 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 servoOut 
-                
-            
- * @param MotorOut 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const int16_t *servoOut,const int16_t *MotorOut)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LLC_OUT_LEN];
-
-	_mav_put_int16_t_array(buf, 0, servoOut, 4);
-	_mav_put_int16_t_array(buf, 8, MotorOut, 2);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LLC_OUT_LEN);
-#else
-	mavlink_llc_out_t packet;
-
-	mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4);
-	mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LLC_OUT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LLC_OUT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LLC_OUT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a llc_out 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 llc_out C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out)
-{
-	return mavlink_msg_llc_out_pack(system_id, component_id, msg, llc_out->servoOut, llc_out->MotorOut);
-}
-
-/**
- * @brief Encode a llc_out 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 llc_out C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_llc_out_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out)
-{
-	return mavlink_msg_llc_out_pack_chan(system_id, component_id, chan, msg, llc_out->servoOut, llc_out->MotorOut);
-}
-
-/**
- * @brief Send a llc_out message
- * @param chan MAVLink channel to send the message
- *
- * @param servoOut 
-                
-            
- * @param MotorOut 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_llc_out_send(mavlink_channel_t chan, const int16_t *servoOut, const int16_t *MotorOut)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LLC_OUT_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE LLC_OUT UNPACKING
-
-
-/**
- * @brief Get field servoOut from llc_out message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_llc_out_get_servoOut(const mavlink_message_t* msg, int16_t *servoOut)
-{
-	return _MAV_RETURN_int16_t_array(msg, servoOut, 4,  0);
-}
-
-/**
- * @brief Get field MotorOut from llc_out message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_llc_out_get_MotorOut(const mavlink_message_t* msg, int16_t *MotorOut)
-{
-	return _MAV_RETURN_int16_t_array(msg, MotorOut, 2,  8);
-}
-
-/**
- * @brief Decode a llc_out message into a struct
- *
- * @param msg The message to decode
- * @param llc_out C-struct to decode the message contents into
- */
-static inline void mavlink_msg_llc_out_decode(const mavlink_message_t* msg, mavlink_llc_out_t* llc_out)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_llc_out_get_servoOut(msg, llc_out->servoOut);
-	mavlink_msg_llc_out_get_MotorOut(msg, llc_out->MotorOut);
-#else
-	memcpy(llc_out, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LLC_OUT_LEN);
-#endif
-}
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
deleted file mode 100644
index 5d9382326..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h
+++ /dev/null
@@ -1,187 +0,0 @@
-// MESSAGE OBS_AIR_TEMP PACKING
-
-#define MAVLINK_MSG_ID_OBS_AIR_TEMP 183
-
-typedef struct __mavlink_obs_air_temp_t
-{
- float airT; ///< 
-                
-            
-} mavlink_obs_air_temp_t;
-
-#define MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN 4
-#define MAVLINK_MSG_ID_183_LEN 4
-
-#define MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC 248
-#define MAVLINK_MSG_ID_183_CRC 248
-
-
-
-#define MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP { \
-	"OBS_AIR_TEMP", \
-	1, \
-	{  { "airT", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_temp_t, airT) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a obs_air_temp 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 airT 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float airT)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN];
-	_mav_put_float(buf, 0, airT);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
-#else
-	mavlink_obs_air_temp_t packet;
-	packet.airT = airT;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
-#endif
-}
-
-/**
- * @brief Pack a obs_air_temp 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 airT 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float airT)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN];
-	_mav_put_float(buf, 0, airT);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
-#else
-	mavlink_obs_air_temp_t packet;
-	packet.airT = airT;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
-#endif
-}
-
-/**
- * @brief Encode a obs_air_temp 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 obs_air_temp C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp)
-{
-	return mavlink_msg_obs_air_temp_pack(system_id, component_id, msg, obs_air_temp->airT);
-}
-
-/**
- * @brief Encode a obs_air_temp 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 obs_air_temp C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_air_temp_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp)
-{
-	return mavlink_msg_obs_air_temp_pack_chan(system_id, component_id, chan, msg, obs_air_temp->airT);
-}
-
-/**
- * @brief Send a obs_air_temp message
- * @param chan MAVLink channel to send the message
- *
- * @param airT 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float airT)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN];
-	_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;
-	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
-
-// MESSAGE OBS_AIR_TEMP UNPACKING
-
-
-/**
- * @brief Get field airT from obs_air_temp message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_obs_air_temp_get_airT(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Decode a obs_air_temp message into a struct
- *
- * @param msg The message to decode
- * @param obs_air_temp C-struct to decode the message contents into
- */
-static inline void mavlink_msg_obs_air_temp_decode(const mavlink_message_t* msg, mavlink_obs_air_temp_t* obs_air_temp)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	obs_air_temp->airT = mavlink_msg_obs_air_temp_get_airT(msg);
-#else
-	memcpy(obs_air_temp, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
-#endif
-}
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
deleted file mode 100644
index 35d813ca1..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h
+++ /dev/null
@@ -1,251 +0,0 @@
-// MESSAGE OBS_AIR_VELOCITY PACKING
-
-#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY 178
-
-typedef struct __mavlink_obs_air_velocity_t
-{
- float magnitude; ///< 
-                
-            
- float aoa; ///< 
-                
-            
- float slip; ///< 
-                
-            
-} mavlink_obs_air_velocity_t;
-
-#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN 12
-#define MAVLINK_MSG_ID_178_LEN 12
-
-#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC 32
-#define MAVLINK_MSG_ID_178_CRC 32
-
-
-
-#define MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY { \
-	"OBS_AIR_VELOCITY", \
-	3, \
-	{  { "magnitude", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_velocity_t, magnitude) }, \
-         { "aoa", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_obs_air_velocity_t, aoa) }, \
-         { "slip", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_obs_air_velocity_t, slip) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a obs_air_velocity 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 magnitude 
-                
-            
- * @param aoa 
-                
-            
- * @param slip 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float magnitude, float aoa, float slip)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN];
-	_mav_put_float(buf, 0, magnitude);
-	_mav_put_float(buf, 4, aoa);
-	_mav_put_float(buf, 8, slip);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
-#else
-	mavlink_obs_air_velocity_t packet;
-	packet.magnitude = magnitude;
-	packet.aoa = aoa;
-	packet.slip = slip;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
-#endif
-}
-
-/**
- * @brief Pack a obs_air_velocity 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 magnitude 
-                
-            
- * @param aoa 
-                
-            
- * @param slip 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float magnitude,float aoa,float slip)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN];
-	_mav_put_float(buf, 0, magnitude);
-	_mav_put_float(buf, 4, aoa);
-	_mav_put_float(buf, 8, slip);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
-#else
-	mavlink_obs_air_velocity_t packet;
-	packet.magnitude = magnitude;
-	packet.aoa = aoa;
-	packet.slip = slip;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
-#endif
-}
-
-/**
- * @brief Encode a obs_air_velocity 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 obs_air_velocity C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity)
-{
-	return mavlink_msg_obs_air_velocity_pack(system_id, component_id, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip);
-}
-
-/**
- * @brief Encode a obs_air_velocity 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 obs_air_velocity C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_air_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity)
-{
-	return mavlink_msg_obs_air_velocity_pack_chan(system_id, component_id, chan, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip);
-}
-
-/**
- * @brief Send a obs_air_velocity message
- * @param chan MAVLink channel to send the message
- *
- * @param magnitude 
-                
-            
- * @param aoa 
-                
-            
- * @param slip 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_obs_air_velocity_send(mavlink_channel_t chan, float magnitude, float aoa, float slip)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN];
-	_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;
-	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
-
-// MESSAGE OBS_AIR_VELOCITY UNPACKING
-
-
-/**
- * @brief Get field magnitude from obs_air_velocity message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_obs_air_velocity_get_magnitude(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field aoa from obs_air_velocity message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_obs_air_velocity_get_aoa(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field slip from obs_air_velocity message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_obs_air_velocity_get_slip(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Decode a obs_air_velocity message into a struct
- *
- * @param msg The message to decode
- * @param obs_air_velocity C-struct to decode the message contents into
- */
-static inline void mavlink_msg_obs_air_velocity_decode(const mavlink_message_t* msg, mavlink_obs_air_velocity_t* obs_air_velocity)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	obs_air_velocity->magnitude = mavlink_msg_obs_air_velocity_get_magnitude(msg);
-	obs_air_velocity->aoa = mavlink_msg_obs_air_velocity_get_aoa(msg);
-	obs_air_velocity->slip = mavlink_msg_obs_air_velocity_get_slip(msg);
-#else
-	memcpy(obs_air_velocity, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
-#endif
-}
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
deleted file mode 100644
index 9c80cd66e..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h
+++ /dev/null
@@ -1,187 +0,0 @@
-// MESSAGE OBS_ATTITUDE PACKING
-
-#define MAVLINK_MSG_ID_OBS_ATTITUDE 174
-
-typedef struct __mavlink_obs_attitude_t
-{
- double quat[4]; ///< 
-                
-            
-} mavlink_obs_attitude_t;
-
-#define MAVLINK_MSG_ID_OBS_ATTITUDE_LEN 32
-#define MAVLINK_MSG_ID_174_LEN 32
-
-#define MAVLINK_MSG_ID_OBS_ATTITUDE_CRC 146
-#define MAVLINK_MSG_ID_174_CRC 146
-
-#define MAVLINK_MSG_OBS_ATTITUDE_FIELD_QUAT_LEN 4
-
-#define MAVLINK_MESSAGE_INFO_OBS_ATTITUDE { \
-	"OBS_ATTITUDE", \
-	1, \
-	{  { "quat", NULL, MAVLINK_TYPE_DOUBLE, 4, 0, offsetof(mavlink_obs_attitude_t, quat) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a obs_attitude 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 quat 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const double *quat)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN];
-
-	_mav_put_double_array(buf, 0, quat, 4);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
-#else
-	mavlink_obs_attitude_t packet;
-
-	mav_array_memcpy(packet.quat, quat, sizeof(double)*4);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a obs_attitude 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 quat 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const double *quat)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN];
-
-	_mav_put_double_array(buf, 0, quat, 4);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
-#else
-	mavlink_obs_attitude_t packet;
-
-	mav_array_memcpy(packet.quat, quat, sizeof(double)*4);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a obs_attitude 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 obs_attitude C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude)
-{
-	return mavlink_msg_obs_attitude_pack(system_id, component_id, msg, obs_attitude->quat);
-}
-
-/**
- * @brief Encode a obs_attitude 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 obs_attitude C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude)
-{
-	return mavlink_msg_obs_attitude_pack_chan(system_id, component_id, chan, msg, obs_attitude->quat);
-}
-
-/**
- * @brief Send a obs_attitude message
- * @param chan MAVLink channel to send the message
- *
- * @param quat 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_obs_attitude_send(mavlink_channel_t chan, const double *quat)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_ATTITUDE_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE OBS_ATTITUDE UNPACKING
-
-
-/**
- * @brief Get field quat from obs_attitude message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_obs_attitude_get_quat(const mavlink_message_t* msg, double *quat)
-{
-	return _MAV_RETURN_double_array(msg, quat, 4,  0);
-}
-
-/**
- * @brief Decode a obs_attitude message into a struct
- *
- * @param msg The message to decode
- * @param obs_attitude C-struct to decode the message contents into
- */
-static inline void mavlink_msg_obs_attitude_decode(const mavlink_message_t* msg, mavlink_obs_attitude_t* obs_attitude)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_obs_attitude_get_quat(msg, obs_attitude->quat);
-#else
-	memcpy(obs_attitude, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
-#endif
-}
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
deleted file mode 100644
index 24dd43b57..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h
+++ /dev/null
@@ -1,220 +0,0 @@
-// MESSAGE OBS_BIAS PACKING
-
-#define MAVLINK_MSG_ID_OBS_BIAS 180
-
-typedef struct __mavlink_obs_bias_t
-{
- float accBias[3]; ///< 
-                
-            
- float gyroBias[3]; ///< 
-                
-            
-} mavlink_obs_bias_t;
-
-#define MAVLINK_MSG_ID_OBS_BIAS_LEN 24
-#define MAVLINK_MSG_ID_180_LEN 24
-
-#define MAVLINK_MSG_ID_OBS_BIAS_CRC 159
-#define MAVLINK_MSG_ID_180_CRC 159
-
-#define MAVLINK_MSG_OBS_BIAS_FIELD_ACCBIAS_LEN 3
-#define MAVLINK_MSG_OBS_BIAS_FIELD_GYROBIAS_LEN 3
-
-#define MAVLINK_MESSAGE_INFO_OBS_BIAS { \
-	"OBS_BIAS", \
-	2, \
-	{  { "accBias", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_bias_t, accBias) }, \
-         { "gyroBias", NULL, MAVLINK_TYPE_FLOAT, 3, 12, offsetof(mavlink_obs_bias_t, gyroBias) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a obs_bias 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 accBias 
-                
-            
- * @param gyroBias 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const float *accBias, const float *gyroBias)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN];
-
-	_mav_put_float_array(buf, 0, accBias, 3);
-	_mav_put_float_array(buf, 12, gyroBias, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_BIAS_LEN);
-#else
-	mavlink_obs_bias_t packet;
-
-	mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3);
-	mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_BIAS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_BIAS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_BIAS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a obs_bias 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 accBias 
-                
-            
- * @param gyroBias 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const float *accBias,const float *gyroBias)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN];
-
-	_mav_put_float_array(buf, 0, accBias, 3);
-	_mav_put_float_array(buf, 12, gyroBias, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_BIAS_LEN);
-#else
-	mavlink_obs_bias_t packet;
-
-	mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3);
-	mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_BIAS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_BIAS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_BIAS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a obs_bias 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 obs_bias C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias)
-{
-	return mavlink_msg_obs_bias_pack(system_id, component_id, msg, obs_bias->accBias, obs_bias->gyroBias);
-}
-
-/**
- * @brief Encode a obs_bias 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 obs_bias C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_bias_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias)
-{
-	return mavlink_msg_obs_bias_pack_chan(system_id, component_id, chan, msg, obs_bias->accBias, obs_bias->gyroBias);
-}
-
-/**
- * @brief Send a obs_bias message
- * @param chan MAVLink channel to send the message
- *
- * @param accBias 
-                
-            
- * @param gyroBias 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_obs_bias_send(mavlink_channel_t chan, const float *accBias, const float *gyroBias)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_BIAS_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE OBS_BIAS UNPACKING
-
-
-/**
- * @brief Get field accBias from obs_bias message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_obs_bias_get_accBias(const mavlink_message_t* msg, float *accBias)
-{
-	return _MAV_RETURN_float_array(msg, accBias, 3,  0);
-}
-
-/**
- * @brief Get field gyroBias from obs_bias message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_obs_bias_get_gyroBias(const mavlink_message_t* msg, float *gyroBias)
-{
-	return _MAV_RETURN_float_array(msg, gyroBias, 3,  12);
-}
-
-/**
- * @brief Decode a obs_bias message into a struct
- *
- * @param msg The message to decode
- * @param obs_bias C-struct to decode the message contents into
- */
-static inline void mavlink_msg_obs_bias_decode(const mavlink_message_t* msg, mavlink_obs_bias_t* obs_bias)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_obs_bias_get_accBias(msg, obs_bias->accBias);
-	mavlink_msg_obs_bias_get_gyroBias(msg, obs_bias->gyroBias);
-#else
-	memcpy(obs_bias, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_BIAS_LEN);
-#endif
-}
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
deleted file mode 100644
index cfc2fe7e1..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h
+++ /dev/null
@@ -1,251 +0,0 @@
-// MESSAGE OBS_POSITION PACKING
-
-#define MAVLINK_MSG_ID_OBS_POSITION 170
-
-typedef struct __mavlink_obs_position_t
-{
- int32_t lon; ///< 
-                
-            
- int32_t lat; ///< 
-                
-            
- int32_t alt; ///< 
-                
-            
-} mavlink_obs_position_t;
-
-#define MAVLINK_MSG_ID_OBS_POSITION_LEN 12
-#define MAVLINK_MSG_ID_170_LEN 12
-
-#define MAVLINK_MSG_ID_OBS_POSITION_CRC 15
-#define MAVLINK_MSG_ID_170_CRC 15
-
-
-
-#define MAVLINK_MESSAGE_INFO_OBS_POSITION { \
-	"OBS_POSITION", \
-	3, \
-	{  { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_obs_position_t, lon) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_obs_position_t, lat) }, \
-         { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_obs_position_t, alt) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a obs_position 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 lon 
-                
-            
- * @param lat 
-                
-            
- * @param alt 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       int32_t lon, int32_t lat, int32_t alt)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN];
-	_mav_put_int32_t(buf, 0, lon);
-	_mav_put_int32_t(buf, 4, lat);
-	_mav_put_int32_t(buf, 8, alt);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_POSITION_LEN);
-#else
-	mavlink_obs_position_t packet;
-	packet.lon = lon;
-	packet.lat = lat;
-	packet.alt = alt;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_POSITION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_POSITION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_POSITION_LEN);
-#endif
-}
-
-/**
- * @brief Pack a obs_position 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 lon 
-                
-            
- * @param lat 
-                
-            
- * @param alt 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           int32_t lon,int32_t lat,int32_t alt)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN];
-	_mav_put_int32_t(buf, 0, lon);
-	_mav_put_int32_t(buf, 4, lat);
-	_mav_put_int32_t(buf, 8, alt);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_POSITION_LEN);
-#else
-	mavlink_obs_position_t packet;
-	packet.lon = lon;
-	packet.lat = lat;
-	packet.alt = alt;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_POSITION_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_POSITION;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_POSITION_LEN);
-#endif
-}
-
-/**
- * @brief Encode a obs_position 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 obs_position C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position)
-{
-	return mavlink_msg_obs_position_pack(system_id, component_id, msg, obs_position->lon, obs_position->lat, obs_position->alt);
-}
-
-/**
- * @brief Encode a obs_position 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 obs_position C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position)
-{
-	return mavlink_msg_obs_position_pack_chan(system_id, component_id, chan, msg, obs_position->lon, obs_position->lat, obs_position->alt);
-}
-
-/**
- * @brief Send a obs_position message
- * @param chan MAVLink channel to send the message
- *
- * @param lon 
-                
-            
- * @param lat 
-                
-            
- * @param alt 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_POSITION_LEN];
-	_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;
-	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
-
-// MESSAGE OBS_POSITION UNPACKING
-
-
-/**
- * @brief Get field lon from obs_position message
- *
- * @return 
-                
-            
- */
-static inline int32_t mavlink_msg_obs_position_get_lon(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field lat from obs_position message
- *
- * @return 
-                
-            
- */
-static inline int32_t mavlink_msg_obs_position_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field alt from obs_position message
- *
- * @return 
-                
-            
- */
-static inline int32_t mavlink_msg_obs_position_get_alt(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Decode a obs_position message into a struct
- *
- * @param msg The message to decode
- * @param obs_position C-struct to decode the message contents into
- */
-static inline void mavlink_msg_obs_position_decode(const mavlink_message_t* msg, mavlink_obs_position_t* obs_position)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	obs_position->lon = mavlink_msg_obs_position_get_lon(msg);
-	obs_position->lat = mavlink_msg_obs_position_get_lat(msg);
-	obs_position->alt = mavlink_msg_obs_position_get_alt(msg);
-#else
-	memcpy(obs_position, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_POSITION_LEN);
-#endif
-}
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
deleted file mode 100644
index 24e272bf7..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h
+++ /dev/null
@@ -1,187 +0,0 @@
-// MESSAGE OBS_QFF PACKING
-
-#define MAVLINK_MSG_ID_OBS_QFF 182
-
-typedef struct __mavlink_obs_qff_t
-{
- float qff; ///< 
-                
-            
-} mavlink_obs_qff_t;
-
-#define MAVLINK_MSG_ID_OBS_QFF_LEN 4
-#define MAVLINK_MSG_ID_182_LEN 4
-
-#define MAVLINK_MSG_ID_OBS_QFF_CRC 24
-#define MAVLINK_MSG_ID_182_CRC 24
-
-
-
-#define MAVLINK_MESSAGE_INFO_OBS_QFF { \
-	"OBS_QFF", \
-	1, \
-	{  { "qff", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_qff_t, qff) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a obs_qff 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 qff 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float qff)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_QFF_LEN];
-	_mav_put_float(buf, 0, qff);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_QFF_LEN);
-#else
-	mavlink_obs_qff_t packet;
-	packet.qff = qff;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_QFF;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_QFF_LEN);
-#endif
-}
-
-/**
- * @brief Pack a obs_qff 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 qff 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float qff)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_QFF_LEN];
-	_mav_put_float(buf, 0, qff);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_QFF_LEN);
-#else
-	mavlink_obs_qff_t packet;
-	packet.qff = qff;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_QFF_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_QFF;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_QFF_LEN);
-#endif
-}
-
-/**
- * @brief Encode a obs_qff 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 obs_qff C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff)
-{
-	return mavlink_msg_obs_qff_pack(system_id, component_id, msg, obs_qff->qff);
-}
-
-/**
- * @brief Encode a obs_qff 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 obs_qff C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_qff_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff)
-{
-	return mavlink_msg_obs_qff_pack_chan(system_id, component_id, chan, msg, obs_qff->qff);
-}
-
-/**
- * @brief Send a obs_qff message
- * @param chan MAVLink channel to send the message
- *
- * @param qff 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_QFF_LEN];
-	_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;
-	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
-
-// MESSAGE OBS_QFF UNPACKING
-
-
-/**
- * @brief Get field qff from obs_qff message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_obs_qff_get_qff(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Decode a obs_qff message into a struct
- *
- * @param msg The message to decode
- * @param obs_qff C-struct to decode the message contents into
- */
-static inline void mavlink_msg_obs_qff_decode(const mavlink_message_t* msg, mavlink_obs_qff_t* obs_qff)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	obs_qff->qff = mavlink_msg_obs_qff_get_qff(msg);
-#else
-	memcpy(obs_qff, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_QFF_LEN);
-#endif
-}
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
deleted file mode 100644
index 6e3776632..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h
+++ /dev/null
@@ -1,187 +0,0 @@
-// MESSAGE OBS_VELOCITY PACKING
-
-#define MAVLINK_MSG_ID_OBS_VELOCITY 172
-
-typedef struct __mavlink_obs_velocity_t
-{
- float vel[3]; ///< 
-                
-            
-} mavlink_obs_velocity_t;
-
-#define MAVLINK_MSG_ID_OBS_VELOCITY_LEN 12
-#define MAVLINK_MSG_ID_172_LEN 12
-
-#define MAVLINK_MSG_ID_OBS_VELOCITY_CRC 108
-#define MAVLINK_MSG_ID_172_CRC 108
-
-#define MAVLINK_MSG_OBS_VELOCITY_FIELD_VEL_LEN 3
-
-#define MAVLINK_MESSAGE_INFO_OBS_VELOCITY { \
-	"OBS_VELOCITY", \
-	1, \
-	{  { "vel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_velocity_t, vel) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a obs_velocity 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 vel 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const float *vel)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN];
-
-	_mav_put_float_array(buf, 0, vel, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
-#else
-	mavlink_obs_velocity_t packet;
-
-	mav_array_memcpy(packet.vel, vel, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
-#endif
-}
-
-/**
- * @brief Pack a obs_velocity 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 vel 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const float *vel)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN];
-
-	_mav_put_float_array(buf, 0, vel, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
-#else
-	mavlink_obs_velocity_t packet;
-
-	mav_array_memcpy(packet.vel, vel, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
-#endif
-}
-
-/**
- * @brief Encode a obs_velocity 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 obs_velocity C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity)
-{
-	return mavlink_msg_obs_velocity_pack(system_id, component_id, msg, obs_velocity->vel);
-}
-
-/**
- * @brief Encode a obs_velocity 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 obs_velocity C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity)
-{
-	return mavlink_msg_obs_velocity_pack_chan(system_id, component_id, chan, msg, obs_velocity->vel);
-}
-
-/**
- * @brief Send a obs_velocity message
- * @param chan MAVLink channel to send the message
- *
- * @param vel 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const float *vel)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_VELOCITY_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE OBS_VELOCITY UNPACKING
-
-
-/**
- * @brief Get field vel from obs_velocity message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_obs_velocity_get_vel(const mavlink_message_t* msg, float *vel)
-{
-	return _MAV_RETURN_float_array(msg, vel, 3,  0);
-}
-
-/**
- * @brief Decode a obs_velocity message into a struct
- *
- * @param msg The message to decode
- * @param obs_velocity C-struct to decode the message contents into
- */
-static inline void mavlink_msg_obs_velocity_decode(const mavlink_message_t* msg, mavlink_obs_velocity_t* obs_velocity)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_obs_velocity_get_vel(msg, obs_velocity->vel);
-#else
-	memcpy(obs_velocity, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
-#endif
-}
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
deleted file mode 100644
index 55f7cb2ae..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h
+++ /dev/null
@@ -1,187 +0,0 @@
-// MESSAGE OBS_WIND PACKING
-
-#define MAVLINK_MSG_ID_OBS_WIND 176
-
-typedef struct __mavlink_obs_wind_t
-{
- float wind[3]; ///< 
-                
-            
-} mavlink_obs_wind_t;
-
-#define MAVLINK_MSG_ID_OBS_WIND_LEN 12
-#define MAVLINK_MSG_ID_176_LEN 12
-
-#define MAVLINK_MSG_ID_OBS_WIND_CRC 16
-#define MAVLINK_MSG_ID_176_CRC 16
-
-#define MAVLINK_MSG_OBS_WIND_FIELD_WIND_LEN 3
-
-#define MAVLINK_MESSAGE_INFO_OBS_WIND { \
-	"OBS_WIND", \
-	1, \
-	{  { "wind", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_wind_t, wind) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a obs_wind 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 wind 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const float *wind)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_WIND_LEN];
-
-	_mav_put_float_array(buf, 0, wind, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_WIND_LEN);
-#else
-	mavlink_obs_wind_t packet;
-
-	mav_array_memcpy(packet.wind, wind, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_WIND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBS_WIND_LEN);
-#endif
-}
-
-/**
- * @brief Pack a obs_wind 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 wind 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const float *wind)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_WIND_LEN];
-
-	_mav_put_float_array(buf, 0, wind, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBS_WIND_LEN);
-#else
-	mavlink_obs_wind_t packet;
-
-	mav_array_memcpy(packet.wind, wind, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBS_WIND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_OBS_WIND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBS_WIND_LEN);
-#endif
-}
-
-/**
- * @brief Encode a obs_wind 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 obs_wind C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind)
-{
-	return mavlink_msg_obs_wind_pack(system_id, component_id, msg, obs_wind->wind);
-}
-
-/**
- * @brief Encode a obs_wind 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 obs_wind C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_obs_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind)
-{
-	return mavlink_msg_obs_wind_pack_chan(system_id, component_id, chan, msg, obs_wind->wind);
-}
-
-/**
- * @brief Send a obs_wind message
- * @param chan MAVLink channel to send the message
- *
- * @param wind 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float *wind)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_OBS_WIND_LEN];
-
-	_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;
-
-	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
-
-// MESSAGE OBS_WIND UNPACKING
-
-
-/**
- * @brief Get field wind from obs_wind message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_obs_wind_get_wind(const mavlink_message_t* msg, float *wind)
-{
-	return _MAV_RETURN_float_array(msg, wind, 3,  0);
-}
-
-/**
- * @brief Decode a obs_wind message into a struct
- *
- * @param msg The message to decode
- * @param obs_wind C-struct to decode the message contents into
- */
-static inline void mavlink_msg_obs_wind_decode(const mavlink_message_t* msg, mavlink_obs_wind_t* obs_wind)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_obs_wind_get_wind(msg, obs_wind->wind);
-#else
-	memcpy(obs_wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OBS_WIND_LEN);
-#endif
-}
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
deleted file mode 100644
index e0963ece7..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h
+++ /dev/null
@@ -1,245 +0,0 @@
-// MESSAGE PM_ELEC PACKING
-
-#define MAVLINK_MSG_ID_PM_ELEC 188
-
-typedef struct __mavlink_pm_elec_t
-{
- float PwCons; ///< 
-                
-            
- float BatStat; ///< 
-                
-            
- float PwGen[3]; ///< 
-                
-            
-} mavlink_pm_elec_t;
-
-#define MAVLINK_MSG_ID_PM_ELEC_LEN 20
-#define MAVLINK_MSG_ID_188_LEN 20
-
-#define MAVLINK_MSG_ID_PM_ELEC_CRC 170
-#define MAVLINK_MSG_ID_188_CRC 170
-
-#define MAVLINK_MSG_PM_ELEC_FIELD_PWGEN_LEN 3
-
-#define MAVLINK_MESSAGE_INFO_PM_ELEC { \
-	"PM_ELEC", \
-	3, \
-	{  { "PwCons", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pm_elec_t, PwCons) }, \
-         { "BatStat", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_pm_elec_t, BatStat) }, \
-         { "PwGen", NULL, MAVLINK_TYPE_FLOAT, 3, 8, offsetof(mavlink_pm_elec_t, PwGen) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a pm_elec 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 PwCons 
-                
-            
- * @param BatStat 
-                
-            
- * @param PwGen 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float PwCons, float BatStat, const float *PwGen)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_PM_ELEC_LEN];
-	_mav_put_float(buf, 0, PwCons);
-	_mav_put_float(buf, 4, BatStat);
-	_mav_put_float_array(buf, 8, PwGen, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PM_ELEC_LEN);
-#else
-	mavlink_pm_elec_t packet;
-	packet.PwCons = PwCons;
-	packet.BatStat = BatStat;
-	mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PM_ELEC_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PM_ELEC;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PM_ELEC_LEN);
-#endif
-}
-
-/**
- * @brief Pack a pm_elec 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 PwCons 
-                
-            
- * @param BatStat 
-                
-            
- * @param PwGen 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float PwCons,float BatStat,const float *PwGen)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_PM_ELEC_LEN];
-	_mav_put_float(buf, 0, PwCons);
-	_mav_put_float(buf, 4, BatStat);
-	_mav_put_float_array(buf, 8, PwGen, 3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PM_ELEC_LEN);
-#else
-	mavlink_pm_elec_t packet;
-	packet.PwCons = PwCons;
-	packet.BatStat = BatStat;
-	mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PM_ELEC_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_PM_ELEC;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PM_ELEC_LEN);
-#endif
-}
-
-/**
- * @brief Encode a pm_elec 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 pm_elec C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec)
-{
-	return mavlink_msg_pm_elec_pack(system_id, component_id, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen);
-}
-
-/**
- * @brief Encode a pm_elec 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 pm_elec C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_pm_elec_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec)
-{
-	return mavlink_msg_pm_elec_pack_chan(system_id, component_id, chan, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen);
-}
-
-/**
- * @brief Send a pm_elec message
- * @param chan MAVLink channel to send the message
- *
- * @param PwCons 
-                
-            
- * @param BatStat 
-                
-            
- * @param PwGen 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_pm_elec_send(mavlink_channel_t chan, float PwCons, float BatStat, const float *PwGen)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_PM_ELEC_LEN];
-	_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;
-	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
-
-// MESSAGE PM_ELEC UNPACKING
-
-
-/**
- * @brief Get field PwCons from pm_elec message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_pm_elec_get_PwCons(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field BatStat from pm_elec message
- *
- * @return 
-                
-            
- */
-static inline float mavlink_msg_pm_elec_get_BatStat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field PwGen from pm_elec message
- *
- * @return 
-                
-            
- */
-static inline uint16_t mavlink_msg_pm_elec_get_PwGen(const mavlink_message_t* msg, float *PwGen)
-{
-	return _MAV_RETURN_float_array(msg, PwGen, 3,  8);
-}
-
-/**
- * @brief Decode a pm_elec message into a struct
- *
- * @param msg The message to decode
- * @param pm_elec C-struct to decode the message contents into
- */
-static inline void mavlink_msg_pm_elec_decode(const mavlink_message_t* msg, mavlink_pm_elec_t* pm_elec)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	pm_elec->PwCons = mavlink_msg_pm_elec_get_PwCons(msg);
-	pm_elec->BatStat = mavlink_msg_pm_elec_get_BatStat(msg);
-	mavlink_msg_pm_elec_get_PwGen(msg, pm_elec->PwGen);
-#else
-	memcpy(pm_elec, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PM_ELEC_LEN);
-#endif
-}
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
deleted file mode 100644
index 94f3e58bb..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h
+++ /dev/null
@@ -1,283 +0,0 @@
-// MESSAGE SYS_Stat PACKING
-
-#define MAVLINK_MSG_ID_SYS_Stat 190
-
-typedef struct __mavlink_sys_stat_t
-{
- uint8_t gps; ///< 
-                
-            
- uint8_t act; ///< 
-                
-            
- uint8_t mod; ///< 
-                
-            
- uint8_t commRssi; ///< 
-                
-            
-} mavlink_sys_stat_t;
-
-#define MAVLINK_MSG_ID_SYS_Stat_LEN 4
-#define MAVLINK_MSG_ID_190_LEN 4
-
-#define MAVLINK_MSG_ID_SYS_Stat_CRC 157
-#define MAVLINK_MSG_ID_190_CRC 157
-
-
-
-#define MAVLINK_MESSAGE_INFO_SYS_Stat { \
-	"SYS_Stat", \
-	4, \
-	{  { "gps", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_sys_stat_t, gps) }, \
-         { "act", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_sys_stat_t, act) }, \
-         { "mod", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_sys_stat_t, mod) }, \
-         { "commRssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_sys_stat_t, commRssi) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a sys_stat 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 gps 
-                
-            
- * @param act 
-                
-            
- * @param mod 
-                
-            
- * @param commRssi 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SYS_Stat_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_Stat_LEN);
-#else
-	mavlink_sys_stat_t packet;
-	packet.gps = gps;
-	packet.act = act;
-	packet.mod = mod;
-	packet.commRssi = commRssi;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_Stat_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SYS_Stat;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_Stat_LEN);
-#endif
-}
-
-/**
- * @brief Pack a sys_stat 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 gps 
-                
-            
- * @param act 
-                
-            
- * @param mod 
-                
-            
- * @param commRssi 
-                
-            
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t gps,uint8_t act,uint8_t mod,uint8_t commRssi)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SYS_Stat_LEN];
-	_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);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_Stat_LEN);
-#else
-	mavlink_sys_stat_t packet;
-	packet.gps = gps;
-	packet.act = act;
-	packet.mod = mod;
-	packet.commRssi = commRssi;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_Stat_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SYS_Stat;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_Stat_LEN);
-#endif
-}
-
-/**
- * @brief Encode a sys_stat 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 sys_stat C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat)
-{
-	return mavlink_msg_sys_stat_pack(system_id, component_id, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi);
-}
-
-/**
- * @brief Encode a sys_stat 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 sys_stat C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sys_stat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat)
-{
-	return mavlink_msg_sys_stat_pack_chan(system_id, component_id, chan, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi);
-}
-
-/**
- * @brief Send a sys_stat message
- * @param chan MAVLink channel to send the message
- *
- * @param gps 
-                
-            
- * @param act 
-                
-            
- * @param mod 
-                
-            
- * @param commRssi 
-                
-            
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_sys_stat_send(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[MAVLINK_MSG_ID_SYS_Stat_LEN];
-	_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;
-	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
-
-// MESSAGE SYS_Stat UNPACKING
-
-
-/**
- * @brief Get field gps from sys_stat message
- *
- * @return 
-                
-            
- */
-static inline uint8_t mavlink_msg_sys_stat_get_gps(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field act from sys_stat message
- *
- * @return 
-                
-            
- */
-static inline uint8_t mavlink_msg_sys_stat_get_act(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field mod from sys_stat message
- *
- * @return 
-                
-            
- */
-static inline uint8_t mavlink_msg_sys_stat_get_mod(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field commRssi from sys_stat message
- *
- * @return 
-                
-            
- */
-static inline uint8_t mavlink_msg_sys_stat_get_commRssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Decode a sys_stat message into a struct
- *
- * @param msg The message to decode
- * @param sys_stat C-struct to decode the message contents into
- */
-static inline void mavlink_msg_sys_stat_decode(const mavlink_message_t* msg, mavlink_sys_stat_t* sys_stat)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	sys_stat->gps = mavlink_msg_sys_stat_get_gps(msg);
-	sys_stat->act = mavlink_msg_sys_stat_get_act(msg);
-	sys_stat->mod = mavlink_msg_sys_stat_get_mod(msg);
-	sys_stat->commRssi = mavlink_msg_sys_stat_get_commRssi(msg);
-#else
-	memcpy(sys_stat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SYS_Stat_LEN);
-#endif
-}
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
deleted file mode 100644
index 26666b0cc..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol generated from sensesoar.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef SENSESOAR_H
-#define SENSESOAR_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// 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}
-#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}
-#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}}}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_SENSESOAR
-
-// ENUM DEFINITIONS
-
-
-/** @brief  Different flight modes  */
-#ifndef HAVE_ENUM_SENSESOAR_MODE
-#define HAVE_ENUM_SENSESOAR_MODE
-enum SENSESOAR_MODE
-{
-	SENSESOAR_MODE_GLIDING=1, /* Gliding mode with motors off | */
-	SENSESOAR_MODE_AUTONOMOUS=2, /* Autonomous flight | */
-	SENSESOAR_MODE_MANUAL=3, /* RC controlled | */
-	SENSESOAR_MODE_ENUM_END=4, /*  | */
-};
-#endif
-
-#include "../common/common.h"
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_obs_position.h"
-#include "./mavlink_msg_obs_velocity.h"
-#include "./mavlink_msg_obs_attitude.h"
-#include "./mavlink_msg_obs_wind.h"
-#include "./mavlink_msg_obs_air_velocity.h"
-#include "./mavlink_msg_obs_bias.h"
-#include "./mavlink_msg_obs_qff.h"
-#include "./mavlink_msg_obs_air_temp.h"
-#include "./mavlink_msg_filt_rot_vel.h"
-#include "./mavlink_msg_llc_out.h"
-#include "./mavlink_msg_pm_elec.h"
-#include "./mavlink_msg_sys_stat.h"
-#include "./mavlink_msg_cmd_airspeed_chng.h"
-#include "./mavlink_msg_cmd_airspeed_ack.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // SENSESOAR_H
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h b/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h
deleted file mode 100644
index 67ffca799..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/testsuite.h
+++ /dev/null
@@ -1,676 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from sensesoar.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef SENSESOAR_TESTSUITE_H
-#define SENSESOAR_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg);
-static void mavlink_test_sensesoar(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_common(system_id, component_id, last_msg);
-	mavlink_test_sensesoar(system_id, component_id, last_msg);
-}
-#endif
-
-#include "../common/testsuite.h"
-
-
-static void mavlink_test_obs_position(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_obs_position_t packet_in = {
-		963497464,
-	}963497672,
-	}963497880,
-	};
-	mavlink_obs_position_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.lon = packet_in.lon;
-        	packet1.lat = packet_in.lat;
-        	packet1.alt = packet_in.alt;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_position_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_obs_position_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_position_pack(system_id, component_id, &msg , packet1.lon , packet1.lat , packet1.alt );
-	mavlink_msg_obs_position_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_position_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.lon , packet1.lat , packet1.alt );
-	mavlink_msg_obs_position_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_obs_position_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_position_send(MAVLINK_COMM_1 , packet1.lon , packet1.lat , packet1.alt );
-	mavlink_msg_obs_position_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_obs_velocity(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_obs_velocity_t packet_in = {
-		{ 17.0, 18.0, 19.0 },
-	};
-	mavlink_obs_velocity_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.vel, packet_in.vel, sizeof(float)*3);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_velocity_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_obs_velocity_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_velocity_pack(system_id, component_id, &msg , packet1.vel );
-	mavlink_msg_obs_velocity_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_velocity_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.vel );
-	mavlink_msg_obs_velocity_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_obs_velocity_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_velocity_send(MAVLINK_COMM_1 , packet1.vel );
-	mavlink_msg_obs_velocity_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_obs_attitude(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_obs_attitude_t packet_in = {
-		{ 123.0, 124.0, 125.0, 126.0 },
-	};
-	mavlink_obs_attitude_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.quat, packet_in.quat, sizeof(double)*4);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_attitude_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_obs_attitude_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_attitude_pack(system_id, component_id, &msg , packet1.quat );
-	mavlink_msg_obs_attitude_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.quat );
-	mavlink_msg_obs_attitude_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_obs_attitude_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_attitude_send(MAVLINK_COMM_1 , packet1.quat );
-	mavlink_msg_obs_attitude_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_obs_wind(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_obs_wind_t packet_in = {
-		{ 17.0, 18.0, 19.0 },
-	};
-	mavlink_obs_wind_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.wind, packet_in.wind, sizeof(float)*3);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_wind_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_obs_wind_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_wind_pack(system_id, component_id, &msg , packet1.wind );
-	mavlink_msg_obs_wind_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_wind_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.wind );
-	mavlink_msg_obs_wind_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_obs_wind_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_wind_send(MAVLINK_COMM_1 , packet1.wind );
-	mavlink_msg_obs_wind_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_obs_air_velocity(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_obs_air_velocity_t packet_in = {
-		17.0,
-	}45.0,
-	}73.0,
-	};
-	mavlink_obs_air_velocity_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.magnitude = packet_in.magnitude;
-        	packet1.aoa = packet_in.aoa;
-        	packet1.slip = packet_in.slip;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_air_velocity_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_obs_air_velocity_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_air_velocity_pack(system_id, component_id, &msg , packet1.magnitude , packet1.aoa , packet1.slip );
-	mavlink_msg_obs_air_velocity_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_air_velocity_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.magnitude , packet1.aoa , packet1.slip );
-	mavlink_msg_obs_air_velocity_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_obs_air_velocity_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_air_velocity_send(MAVLINK_COMM_1 , packet1.magnitude , packet1.aoa , packet1.slip );
-	mavlink_msg_obs_air_velocity_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_obs_bias(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_obs_bias_t packet_in = {
-		{ 17.0, 18.0, 19.0 },
-	}{ 101.0, 102.0, 103.0 },
-	};
-	mavlink_obs_bias_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.accBias, packet_in.accBias, sizeof(float)*3);
-        	mav_array_memcpy(packet1.gyroBias, packet_in.gyroBias, sizeof(float)*3);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_bias_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_obs_bias_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_bias_pack(system_id, component_id, &msg , packet1.accBias , packet1.gyroBias );
-	mavlink_msg_obs_bias_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_bias_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.accBias , packet1.gyroBias );
-	mavlink_msg_obs_bias_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_obs_bias_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_bias_send(MAVLINK_COMM_1 , packet1.accBias , packet1.gyroBias );
-	mavlink_msg_obs_bias_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_obs_qff(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_obs_qff_t packet_in = {
-		17.0,
-	};
-	mavlink_obs_qff_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.qff = packet_in.qff;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_qff_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_obs_qff_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_qff_pack(system_id, component_id, &msg , packet1.qff );
-	mavlink_msg_obs_qff_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_qff_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.qff );
-	mavlink_msg_obs_qff_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_obs_qff_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_qff_send(MAVLINK_COMM_1 , packet1.qff );
-	mavlink_msg_obs_qff_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_obs_air_temp(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_obs_air_temp_t packet_in = {
-		17.0,
-	};
-	mavlink_obs_air_temp_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.airT = packet_in.airT;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_air_temp_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_obs_air_temp_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_air_temp_pack(system_id, component_id, &msg , packet1.airT );
-	mavlink_msg_obs_air_temp_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_air_temp_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.airT );
-	mavlink_msg_obs_air_temp_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_obs_air_temp_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_obs_air_temp_send(MAVLINK_COMM_1 , packet1.airT );
-	mavlink_msg_obs_air_temp_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_filt_rot_vel(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_filt_rot_vel_t packet_in = {
-		{ 17.0, 18.0, 19.0 },
-	};
-	mavlink_filt_rot_vel_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.rotVel, packet_in.rotVel, sizeof(float)*3);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_filt_rot_vel_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_filt_rot_vel_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_filt_rot_vel_pack(system_id, component_id, &msg , packet1.rotVel );
-	mavlink_msg_filt_rot_vel_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_filt_rot_vel_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rotVel );
-	mavlink_msg_filt_rot_vel_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_filt_rot_vel_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_filt_rot_vel_send(MAVLINK_COMM_1 , packet1.rotVel );
-	mavlink_msg_filt_rot_vel_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_llc_out(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_llc_out_t packet_in = {
-		{ 17235, 17236, 17237, 17238 },
-	}{ 17651, 17652 },
-	};
-	mavlink_llc_out_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        
-        	mav_array_memcpy(packet1.servoOut, packet_in.servoOut, sizeof(int16_t)*4);
-        	mav_array_memcpy(packet1.MotorOut, packet_in.MotorOut, sizeof(int16_t)*2);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_llc_out_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_llc_out_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_llc_out_pack(system_id, component_id, &msg , packet1.servoOut , packet1.MotorOut );
-	mavlink_msg_llc_out_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_llc_out_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.servoOut , packet1.MotorOut );
-	mavlink_msg_llc_out_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_llc_out_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_llc_out_send(MAVLINK_COMM_1 , packet1.servoOut , packet1.MotorOut );
-	mavlink_msg_llc_out_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_pm_elec(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_pm_elec_t packet_in = {
-		17.0,
-	}45.0,
-	}{ 73.0, 74.0, 75.0 },
-	};
-	mavlink_pm_elec_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.PwCons = packet_in.PwCons;
-        	packet1.BatStat = packet_in.BatStat;
-        
-        	mav_array_memcpy(packet1.PwGen, packet_in.PwGen, sizeof(float)*3);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_pm_elec_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_pm_elec_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_pm_elec_pack(system_id, component_id, &msg , packet1.PwCons , packet1.BatStat , packet1.PwGen );
-	mavlink_msg_pm_elec_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_pm_elec_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.PwCons , packet1.BatStat , packet1.PwGen );
-	mavlink_msg_pm_elec_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_pm_elec_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_pm_elec_send(MAVLINK_COMM_1 , packet1.PwCons , packet1.BatStat , packet1.PwGen );
-	mavlink_msg_pm_elec_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_sys_stat(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_sys_stat_t packet_in = {
-		5,
-	}72,
-	}139,
-	}206,
-	};
-	mavlink_sys_stat_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.gps = packet_in.gps;
-        	packet1.act = packet_in.act;
-        	packet1.mod = packet_in.mod;
-        	packet1.commRssi = packet_in.commRssi;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sys_stat_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_sys_stat_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sys_stat_pack(system_id, component_id, &msg , packet1.gps , packet1.act , packet1.mod , packet1.commRssi );
-	mavlink_msg_sys_stat_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sys_stat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.gps , packet1.act , packet1.mod , packet1.commRssi );
-	mavlink_msg_sys_stat_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_sys_stat_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sys_stat_send(MAVLINK_COMM_1 , packet1.gps , packet1.act , packet1.mod , packet1.commRssi );
-	mavlink_msg_sys_stat_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_cmd_airspeed_chng(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_cmd_airspeed_chng_t packet_in = {
-		17.0,
-	}17,
-	};
-	mavlink_cmd_airspeed_chng_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.spCmd = packet_in.spCmd;
-        	packet1.target = packet_in.target;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_cmd_airspeed_chng_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_cmd_airspeed_chng_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_cmd_airspeed_chng_pack(system_id, component_id, &msg , packet1.target , packet1.spCmd );
-	mavlink_msg_cmd_airspeed_chng_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_cmd_airspeed_chng_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target , packet1.spCmd );
-	mavlink_msg_cmd_airspeed_chng_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_cmd_airspeed_chng_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_cmd_airspeed_chng_send(MAVLINK_COMM_1 , packet1.target , packet1.spCmd );
-	mavlink_msg_cmd_airspeed_chng_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_cmd_airspeed_ack(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_cmd_airspeed_ack_t packet_in = {
-		17.0,
-	}17,
-	};
-	mavlink_cmd_airspeed_ack_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.spCmd = packet_in.spCmd;
-        	packet1.ack = packet_in.ack;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_cmd_airspeed_ack_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_cmd_airspeed_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_cmd_airspeed_ack_pack(system_id, component_id, &msg , packet1.spCmd , packet1.ack );
-	mavlink_msg_cmd_airspeed_ack_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_cmd_airspeed_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.spCmd , packet1.ack );
-	mavlink_msg_cmd_airspeed_ack_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_cmd_airspeed_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_cmd_airspeed_ack_send(MAVLINK_COMM_1 , packet1.spCmd , packet1.ack );
-	mavlink_msg_cmd_airspeed_ack_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_sensesoar(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_obs_position(system_id, component_id, last_msg);
-	mavlink_test_obs_velocity(system_id, component_id, last_msg);
-	mavlink_test_obs_attitude(system_id, component_id, last_msg);
-	mavlink_test_obs_wind(system_id, component_id, last_msg);
-	mavlink_test_obs_air_velocity(system_id, component_id, last_msg);
-	mavlink_test_obs_bias(system_id, component_id, last_msg);
-	mavlink_test_obs_qff(system_id, component_id, last_msg);
-	mavlink_test_obs_air_temp(system_id, component_id, last_msg);
-	mavlink_test_filt_rot_vel(system_id, component_id, last_msg);
-	mavlink_test_llc_out(system_id, component_id, last_msg);
-	mavlink_test_pm_elec(system_id, component_id, last_msg);
-	mavlink_test_sys_stat(system_id, component_id, last_msg);
-	mavlink_test_cmd_airspeed_chng(system_id, component_id, last_msg);
-	mavlink_test_cmd_airspeed_ack(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // SENSESOAR_TESTSUITE_H
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h
deleted file mode 100644
index cdd683949..000000000
--- a/mavlink/include/mavlink/v1.0/sensesoar/version.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from sensesoar.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:27 2014"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
- 
-#endif // MAVLINK_VERSION_H
diff --git a/mavlink/lib/pkgconfig/mavlink.pc b/mavlink/lib/pkgconfig/mavlink.pc
deleted file mode 100644
index be9036495..000000000
--- a/mavlink/lib/pkgconfig/mavlink.pc
+++ /dev/null
@@ -1,7 +0,0 @@
-prefix=/
-exec_prefix=/
-
-Name: mavlink
-Description: 
-Version: 
-Cflags: -I//include/mavlink
diff --git a/mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc b/mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc
deleted file mode 100644
index e984f512a..000000000
--- a/mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc
+++ /dev/null
@@ -1,5431 +0,0 @@
-// Generated by the protocol buffer compiler.  DO NOT EDIT!
-
-#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
-#include "pixhawk.pb.h"
-
-#include <algorithm>
-
-#include <google/protobuf/stubs/once.h>
-#include <google/protobuf/io/coded_stream.h>
-#include <google/protobuf/wire_format_lite_inl.h>
-#include <google/protobuf/descriptor.h>
-#include <google/protobuf/reflection_ops.h>
-#include <google/protobuf/wire_format.h>
-// @@protoc_insertion_point(includes)
-
-namespace px {
-
-namespace {
-
-const ::google::protobuf::Descriptor* HeaderInfo_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  HeaderInfo_reflection_ = NULL;
-const ::google::protobuf::Descriptor* GLOverlay_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  GLOverlay_reflection_ = NULL;
-const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor_ = NULL;
-const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor_ = NULL;
-const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor_ = NULL;
-const ::google::protobuf::Descriptor* Obstacle_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  Obstacle_reflection_ = NULL;
-const ::google::protobuf::Descriptor* ObstacleList_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  ObstacleList_reflection_ = NULL;
-const ::google::protobuf::Descriptor* ObstacleMap_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  ObstacleMap_reflection_ = NULL;
-const ::google::protobuf::Descriptor* Path_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  Path_reflection_ = NULL;
-const ::google::protobuf::Descriptor* PointCloudXYZI_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  PointCloudXYZI_reflection_ = NULL;
-const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  PointCloudXYZI_PointXYZI_reflection_ = NULL;
-const ::google::protobuf::Descriptor* PointCloudXYZRGB_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  PointCloudXYZRGB_reflection_ = NULL;
-const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  PointCloudXYZRGB_PointXYZRGB_reflection_ = NULL;
-const ::google::protobuf::Descriptor* RGBDImage_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  RGBDImage_reflection_ = NULL;
-const ::google::protobuf::Descriptor* Waypoint_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  Waypoint_reflection_ = NULL;
-
-}  // namespace
-
-
-void protobuf_AssignDesc_pixhawk_2eproto() {
-  protobuf_AddDesc_pixhawk_2eproto();
-  const ::google::protobuf::FileDescriptor* file =
-    ::google::protobuf::DescriptorPool::generated_pool()->FindFileByName(
-      "pixhawk.proto");
-  GOOGLE_CHECK(file != NULL);
-  HeaderInfo_descriptor_ = file->message_type(0);
-  static const int HeaderInfo_offsets_[3] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, source_sysid_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, source_compid_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, timestamp_),
-  };
-  HeaderInfo_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      HeaderInfo_descriptor_,
-      HeaderInfo::default_instance_,
-      HeaderInfo_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(HeaderInfo));
-  GLOverlay_descriptor_ = file->message_type(1);
-  static const int GLOverlay_offsets_[7] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, name_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, coordinateframetype_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, data_),
-  };
-  GLOverlay_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      GLOverlay_descriptor_,
-      GLOverlay::default_instance_,
-      GLOverlay_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(GLOverlay));
-  GLOverlay_CoordinateFrameType_descriptor_ = GLOverlay_descriptor_->enum_type(0);
-  GLOverlay_Mode_descriptor_ = GLOverlay_descriptor_->enum_type(1);
-  GLOverlay_Identifier_descriptor_ = GLOverlay_descriptor_->enum_type(2);
-  Obstacle_descriptor_ = file->message_type(2);
-  static const int Obstacle_offsets_[6] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, length_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, width_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, height_),
-  };
-  Obstacle_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      Obstacle_descriptor_,
-      Obstacle::default_instance_,
-      Obstacle_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(Obstacle));
-  ObstacleList_descriptor_ = file->message_type(3);
-  static const int ObstacleList_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, obstacles_),
-  };
-  ObstacleList_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      ObstacleList_descriptor_,
-      ObstacleList::default_instance_,
-      ObstacleList_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(ObstacleList));
-  ObstacleMap_descriptor_ = file->message_type(4);
-  static const int ObstacleMap_offsets_[10] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, type_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, resolution_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, rows_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, cols_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapr0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapc0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayr0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayc0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, data_),
-  };
-  ObstacleMap_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      ObstacleMap_descriptor_,
-      ObstacleMap::default_instance_,
-      ObstacleMap_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(ObstacleMap));
-  Path_descriptor_ = file->message_type(5);
-  static const int Path_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, waypoints_),
-  };
-  Path_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      Path_descriptor_,
-      Path::default_instance_,
-      Path_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(Path));
-  PointCloudXYZI_descriptor_ = file->message_type(6);
-  static const int PointCloudXYZI_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, points_),
-  };
-  PointCloudXYZI_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      PointCloudXYZI_descriptor_,
-      PointCloudXYZI::default_instance_,
-      PointCloudXYZI_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(PointCloudXYZI));
-  PointCloudXYZI_PointXYZI_descriptor_ = PointCloudXYZI_descriptor_->nested_type(0);
-  static const int PointCloudXYZI_PointXYZI_offsets_[4] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, intensity_),
-  };
-  PointCloudXYZI_PointXYZI_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      PointCloudXYZI_PointXYZI_descriptor_,
-      PointCloudXYZI_PointXYZI::default_instance_,
-      PointCloudXYZI_PointXYZI_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(PointCloudXYZI_PointXYZI));
-  PointCloudXYZRGB_descriptor_ = file->message_type(7);
-  static const int PointCloudXYZRGB_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, points_),
-  };
-  PointCloudXYZRGB_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      PointCloudXYZRGB_descriptor_,
-      PointCloudXYZRGB::default_instance_,
-      PointCloudXYZRGB_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(PointCloudXYZRGB));
-  PointCloudXYZRGB_PointXYZRGB_descriptor_ = PointCloudXYZRGB_descriptor_->nested_type(0);
-  static const int PointCloudXYZRGB_PointXYZRGB_offsets_[4] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, rgb_),
-  };
-  PointCloudXYZRGB_PointXYZRGB_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      PointCloudXYZRGB_PointXYZRGB_descriptor_,
-      PointCloudXYZRGB_PointXYZRGB::default_instance_,
-      PointCloudXYZRGB_PointXYZRGB_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(PointCloudXYZRGB_PointXYZRGB));
-  RGBDImage_descriptor_ = file->message_type(8);
-  static const int RGBDImage_offsets_[21] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, cols_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, rows_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, step1_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, type1_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, imagedata1_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, step2_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, type2_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, imagedata2_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_config_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_type_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, roll_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, pitch_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, yaw_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, lon_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, lat_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, alt_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_matrix_),
-  };
-  RGBDImage_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      RGBDImage_descriptor_,
-      RGBDImage::default_instance_,
-      RGBDImage_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(RGBDImage));
-  Waypoint_descriptor_ = file->message_type(9);
-  static const int Waypoint_offsets_[6] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, roll_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, pitch_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, yaw_),
-  };
-  Waypoint_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      Waypoint_descriptor_,
-      Waypoint::default_instance_,
-      Waypoint_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(Waypoint));
-}
-
-namespace {
-
-GOOGLE_PROTOBUF_DECLARE_ONCE(protobuf_AssignDescriptors_once_);
-inline void protobuf_AssignDescriptorsOnce() {
-  ::google::protobuf::GoogleOnceInit(&protobuf_AssignDescriptors_once_,
-                 &protobuf_AssignDesc_pixhawk_2eproto);
-}
-
-void protobuf_RegisterTypes(const ::std::string&) {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    HeaderInfo_descriptor_, &HeaderInfo::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    GLOverlay_descriptor_, &GLOverlay::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    Obstacle_descriptor_, &Obstacle::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    ObstacleList_descriptor_, &ObstacleList::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    ObstacleMap_descriptor_, &ObstacleMap::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    Path_descriptor_, &Path::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    PointCloudXYZI_descriptor_, &PointCloudXYZI::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    PointCloudXYZI_PointXYZI_descriptor_, &PointCloudXYZI_PointXYZI::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    PointCloudXYZRGB_descriptor_, &PointCloudXYZRGB::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    PointCloudXYZRGB_PointXYZRGB_descriptor_, &PointCloudXYZRGB_PointXYZRGB::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    RGBDImage_descriptor_, &RGBDImage::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    Waypoint_descriptor_, &Waypoint::default_instance());
-}
-
-}  // namespace
-
-void protobuf_ShutdownFile_pixhawk_2eproto() {
-  delete HeaderInfo::default_instance_;
-  delete HeaderInfo_reflection_;
-  delete GLOverlay::default_instance_;
-  delete GLOverlay_reflection_;
-  delete Obstacle::default_instance_;
-  delete Obstacle_reflection_;
-  delete ObstacleList::default_instance_;
-  delete ObstacleList_reflection_;
-  delete ObstacleMap::default_instance_;
-  delete ObstacleMap_reflection_;
-  delete Path::default_instance_;
-  delete Path_reflection_;
-  delete PointCloudXYZI::default_instance_;
-  delete PointCloudXYZI_reflection_;
-  delete PointCloudXYZI_PointXYZI::default_instance_;
-  delete PointCloudXYZI_PointXYZI_reflection_;
-  delete PointCloudXYZRGB::default_instance_;
-  delete PointCloudXYZRGB_reflection_;
-  delete PointCloudXYZRGB_PointXYZRGB::default_instance_;
-  delete PointCloudXYZRGB_PointXYZRGB_reflection_;
-  delete RGBDImage::default_instance_;
-  delete RGBDImage_reflection_;
-  delete Waypoint::default_instance_;
-  delete Waypoint_reflection_;
-}
-
-void protobuf_AddDesc_pixhawk_2eproto() {
-  static bool already_here = false;
-  if (already_here) return;
-  already_here = true;
-  GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-  ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-    "\n\rpixhawk.proto\022\002px\"L\n\nHeaderInfo\022\024\n\014sou"
-    "rce_sysid\030\001 \002(\005\022\025\n\rsource_compid\030\002 \002(\005\022\021"
-    "\n\ttimestamp\030\003 \002(\001\"\377\004\n\tGLOverlay\022\036\n\006heade"
-    "r\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004name\030\002 \001(\t\022>\n"
-    "\023coordinateFrameType\030\003 \001(\0162!.px.GLOverla"
-    "y.CoordinateFrameType\022\020\n\010origin_x\030\004 \001(\001\022"
-    "\020\n\010origin_y\030\005 \001(\001\022\020\n\010origin_z\030\006 \001(\001\022\014\n\004d"
-    "ata\030\007 \001(\014\",\n\023CoordinateFrameType\022\n\n\006GLOB"
-    "AL\020\000\022\t\n\005LOCAL\020\001\"\333\001\n\004Mode\022\n\n\006POINTS\020\000\022\t\n\005"
-    "LINES\020\001\022\016\n\nLINE_STRIP\020\002\022\r\n\tLINE_LOOP\020\003\022\r"
-    "\n\tTRIANGLES\020\004\022\022\n\016TRIANGLE_STRIP\020\005\022\020\n\014TRI"
-    "ANGLE_FAN\020\006\022\t\n\005QUADS\020\007\022\016\n\nQUAD_STRIP\020\010\022\013"
-    "\n\007POLYGON\020\t\022\020\n\014SOLID_CIRCLE\020\n\022\017\n\013WIRE_CI"
-    "RCLE\020\013\022\016\n\nSOLID_CUBE\020\014\022\r\n\tWIRE_CUBE\020\r\"\263\001"
-    "\n\nIdentifier\022\007\n\003END\020\016\022\014\n\010VERTEX2F\020\017\022\014\n\010V"
-    "ERTEX3F\020\020\022\013\n\007ROTATEF\020\021\022\016\n\nTRANSLATEF\020\022\022\n"
-    "\n\006SCALEF\020\023\022\017\n\013PUSH_MATRIX\020\024\022\016\n\nPOP_MATRI"
-    "X\020\025\022\013\n\007COLOR3F\020\026\022\013\n\007COLOR4F\020\027\022\r\n\tPOINTSI"
-    "ZE\020\030\022\r\n\tLINEWIDTH\020\031\"Z\n\010Obstacle\022\t\n\001x\030\001 \001"
-    "(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001z\030\003 \001(\002\022\016\n\006length\030\004 \001(\002"
-    "\022\r\n\005width\030\005 \001(\002\022\016\n\006height\030\006 \001(\002\"O\n\014Obsta"
-    "cleList\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo\022"
-    "\037\n\tobstacles\030\002 \003(\0132\014.px.Obstacle\"\271\001\n\013Obs"
-    "tacleMap\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo"
-    "\022\014\n\004type\030\002 \002(\005\022\022\n\nresolution\030\003 \001(\002\022\014\n\004ro"
-    "ws\030\004 \001(\005\022\014\n\004cols\030\005 \001(\005\022\r\n\005mapR0\030\006 \001(\005\022\r\n"
-    "\005mapC0\030\007 \001(\005\022\017\n\007arrayR0\030\010 \001(\005\022\017\n\007arrayC0"
-    "\030\t \001(\005\022\014\n\004data\030\n \001(\014\"G\n\004Path\022\036\n\006header\030\001"
-    " \002(\0132\016.px.HeaderInfo\022\037\n\twaypoints\030\002 \003(\0132"
-    "\014.px.Waypoint\"\237\001\n\016PointCloudXYZI\022\036\n\006head"
-    "er\030\001 \002(\0132\016.px.HeaderInfo\022,\n\006points\030\002 \003(\013"
-    "2\034.px.PointCloudXYZI.PointXYZI\032\?\n\tPointX"
-    "YZI\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z\030\003 \002(\002\022\021\n\t"
-    "intensity\030\004 \002(\002\"\241\001\n\020PointCloudXYZRGB\022\036\n\006"
-    "header\030\001 \002(\0132\016.px.HeaderInfo\0220\n\006points\030\002"
-    " \003(\0132 .px.PointCloudXYZRGB.PointXYZRGB\032;"
-    "\n\013PointXYZRGB\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z"
-    "\030\003 \002(\002\022\013\n\003rgb\030\004 \002(\002\"\365\002\n\tRGBDImage\022\036\n\006hea"
-    "der\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004cols\030\002 \002(\r\022"
-    "\014\n\004rows\030\003 \002(\r\022\r\n\005step1\030\004 \002(\r\022\r\n\005type1\030\005 "
-    "\002(\r\022\022\n\nimageData1\030\006 \002(\014\022\r\n\005step2\030\007 \002(\r\022\r"
-    "\n\005type2\030\010 \002(\r\022\022\n\nimageData2\030\t \002(\014\022\025\n\rcam"
-    "era_config\030\n \001(\r\022\023\n\013camera_type\030\013 \001(\r\022\014\n"
-    "\004roll\030\014 \001(\002\022\r\n\005pitch\030\r \001(\002\022\013\n\003yaw\030\016 \001(\002\022"
-    "\013\n\003lon\030\017 \001(\002\022\013\n\003lat\030\020 \001(\002\022\013\n\003alt\030\021 \001(\002\022\020"
-    "\n\010ground_x\030\022 \001(\002\022\020\n\010ground_y\030\023 \001(\002\022\020\n\010gr"
-    "ound_z\030\024 \001(\002\022\025\n\rcamera_matrix\030\025 \003(\002\"U\n\010W"
-    "aypoint\022\t\n\001x\030\001 \002(\001\022\t\n\001y\030\002 \002(\001\022\t\n\001z\030\003 \001(\001"
-    "\022\014\n\004roll\030\004 \001(\001\022\r\n\005pitch\030\005 \001(\001\022\013\n\003yaw\030\006 \001"
-    "(\001", 1962);
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
-    "pixhawk.proto", &protobuf_RegisterTypes);
-  HeaderInfo::default_instance_ = new HeaderInfo();
-  GLOverlay::default_instance_ = new GLOverlay();
-  Obstacle::default_instance_ = new Obstacle();
-  ObstacleList::default_instance_ = new ObstacleList();
-  ObstacleMap::default_instance_ = new ObstacleMap();
-  Path::default_instance_ = new Path();
-  PointCloudXYZI::default_instance_ = new PointCloudXYZI();
-  PointCloudXYZI_PointXYZI::default_instance_ = new PointCloudXYZI_PointXYZI();
-  PointCloudXYZRGB::default_instance_ = new PointCloudXYZRGB();
-  PointCloudXYZRGB_PointXYZRGB::default_instance_ = new PointCloudXYZRGB_PointXYZRGB();
-  RGBDImage::default_instance_ = new RGBDImage();
-  Waypoint::default_instance_ = new Waypoint();
-  HeaderInfo::default_instance_->InitAsDefaultInstance();
-  GLOverlay::default_instance_->InitAsDefaultInstance();
-  Obstacle::default_instance_->InitAsDefaultInstance();
-  ObstacleList::default_instance_->InitAsDefaultInstance();
-  ObstacleMap::default_instance_->InitAsDefaultInstance();
-  Path::default_instance_->InitAsDefaultInstance();
-  PointCloudXYZI::default_instance_->InitAsDefaultInstance();
-  PointCloudXYZI_PointXYZI::default_instance_->InitAsDefaultInstance();
-  PointCloudXYZRGB::default_instance_->InitAsDefaultInstance();
-  PointCloudXYZRGB_PointXYZRGB::default_instance_->InitAsDefaultInstance();
-  RGBDImage::default_instance_->InitAsDefaultInstance();
-  Waypoint::default_instance_->InitAsDefaultInstance();
-  ::google::protobuf::internal::OnShutdown(&protobuf_ShutdownFile_pixhawk_2eproto);
-}
-
-// Force AddDescriptors() to be called at static initialization time.
-struct StaticDescriptorInitializer_pixhawk_2eproto {
-  StaticDescriptorInitializer_pixhawk_2eproto() {
-    protobuf_AddDesc_pixhawk_2eproto();
-  }
-} static_descriptor_initializer_pixhawk_2eproto_;
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int HeaderInfo::kSourceSysidFieldNumber;
-const int HeaderInfo::kSourceCompidFieldNumber;
-const int HeaderInfo::kTimestampFieldNumber;
-#endif  // !_MSC_VER
-
-HeaderInfo::HeaderInfo()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void HeaderInfo::InitAsDefaultInstance() {
-}
-
-HeaderInfo::HeaderInfo(const HeaderInfo& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void HeaderInfo::SharedCtor() {
-  _cached_size_ = 0;
-  source_sysid_ = 0;
-  source_compid_ = 0;
-  timestamp_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-HeaderInfo::~HeaderInfo() {
-  SharedDtor();
-}
-
-void HeaderInfo::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void HeaderInfo::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* HeaderInfo::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return HeaderInfo_descriptor_;
-}
-
-const HeaderInfo& HeaderInfo::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-HeaderInfo* HeaderInfo::default_instance_ = NULL;
-
-HeaderInfo* HeaderInfo::New() const {
-  return new HeaderInfo;
-}
-
-void HeaderInfo::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    source_sysid_ = 0;
-    source_compid_ = 0;
-    timestamp_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool HeaderInfo::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required int32 source_sysid = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &source_sysid_)));
-          set_has_source_sysid();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(16)) goto parse_source_compid;
-        break;
-      }
-      
-      // required int32 source_compid = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_source_compid:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &source_compid_)));
-          set_has_source_compid();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(25)) goto parse_timestamp;
-        break;
-      }
-      
-      // required double timestamp = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_timestamp:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &timestamp_)));
-          set_has_timestamp();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void HeaderInfo::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required int32 source_sysid = 1;
-  if (has_source_sysid()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(1, this->source_sysid(), output);
-  }
-  
-  // required int32 source_compid = 2;
-  if (has_source_compid()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->source_compid(), output);
-  }
-  
-  // required double timestamp = 3;
-  if (has_timestamp()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->timestamp(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* HeaderInfo::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required int32 source_sysid = 1;
-  if (has_source_sysid()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(1, this->source_sysid(), target);
-  }
-  
-  // required int32 source_compid = 2;
-  if (has_source_compid()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->source_compid(), target);
-  }
-  
-  // required double timestamp = 3;
-  if (has_timestamp()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->timestamp(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int HeaderInfo::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required int32 source_sysid = 1;
-    if (has_source_sysid()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->source_sysid());
-    }
-    
-    // required int32 source_compid = 2;
-    if (has_source_compid()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->source_compid());
-    }
-    
-    // required double timestamp = 3;
-    if (has_timestamp()) {
-      total_size += 1 + 8;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void HeaderInfo::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const HeaderInfo* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const HeaderInfo*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void HeaderInfo::MergeFrom(const HeaderInfo& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_source_sysid()) {
-      set_source_sysid(from.source_sysid());
-    }
-    if (from.has_source_compid()) {
-      set_source_compid(from.source_compid());
-    }
-    if (from.has_timestamp()) {
-      set_timestamp(from.timestamp());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void HeaderInfo::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void HeaderInfo::CopyFrom(const HeaderInfo& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool HeaderInfo::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000007) != 0x00000007) return false;
-  
-  return true;
-}
-
-void HeaderInfo::Swap(HeaderInfo* other) {
-  if (other != this) {
-    std::swap(source_sysid_, other->source_sysid_);
-    std::swap(source_compid_, other->source_compid_);
-    std::swap(timestamp_, other->timestamp_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata HeaderInfo::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = HeaderInfo_descriptor_;
-  metadata.reflection = HeaderInfo_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return GLOverlay_CoordinateFrameType_descriptor_;
-}
-bool GLOverlay_CoordinateFrameType_IsValid(int value) {
-  switch(value) {
-    case 0:
-    case 1:
-      return true;
-    default:
-      return false;
-  }
-}
-
-#ifndef _MSC_VER
-const GLOverlay_CoordinateFrameType GLOverlay::GLOBAL;
-const GLOverlay_CoordinateFrameType GLOverlay::LOCAL;
-const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MIN;
-const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MAX;
-const int GLOverlay::CoordinateFrameType_ARRAYSIZE;
-#endif  // _MSC_VER
-const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return GLOverlay_Mode_descriptor_;
-}
-bool GLOverlay_Mode_IsValid(int value) {
-  switch(value) {
-    case 0:
-    case 1:
-    case 2:
-    case 3:
-    case 4:
-    case 5:
-    case 6:
-    case 7:
-    case 8:
-    case 9:
-    case 10:
-    case 11:
-    case 12:
-    case 13:
-      return true;
-    default:
-      return false;
-  }
-}
-
-#ifndef _MSC_VER
-const GLOverlay_Mode GLOverlay::POINTS;
-const GLOverlay_Mode GLOverlay::LINES;
-const GLOverlay_Mode GLOverlay::LINE_STRIP;
-const GLOverlay_Mode GLOverlay::LINE_LOOP;
-const GLOverlay_Mode GLOverlay::TRIANGLES;
-const GLOverlay_Mode GLOverlay::TRIANGLE_STRIP;
-const GLOverlay_Mode GLOverlay::TRIANGLE_FAN;
-const GLOverlay_Mode GLOverlay::QUADS;
-const GLOverlay_Mode GLOverlay::QUAD_STRIP;
-const GLOverlay_Mode GLOverlay::POLYGON;
-const GLOverlay_Mode GLOverlay::SOLID_CIRCLE;
-const GLOverlay_Mode GLOverlay::WIRE_CIRCLE;
-const GLOverlay_Mode GLOverlay::SOLID_CUBE;
-const GLOverlay_Mode GLOverlay::WIRE_CUBE;
-const GLOverlay_Mode GLOverlay::Mode_MIN;
-const GLOverlay_Mode GLOverlay::Mode_MAX;
-const int GLOverlay::Mode_ARRAYSIZE;
-#endif  // _MSC_VER
-const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return GLOverlay_Identifier_descriptor_;
-}
-bool GLOverlay_Identifier_IsValid(int value) {
-  switch(value) {
-    case 14:
-    case 15:
-    case 16:
-    case 17:
-    case 18:
-    case 19:
-    case 20:
-    case 21:
-    case 22:
-    case 23:
-    case 24:
-    case 25:
-      return true;
-    default:
-      return false;
-  }
-}
-
-#ifndef _MSC_VER
-const GLOverlay_Identifier GLOverlay::END;
-const GLOverlay_Identifier GLOverlay::VERTEX2F;
-const GLOverlay_Identifier GLOverlay::VERTEX3F;
-const GLOverlay_Identifier GLOverlay::ROTATEF;
-const GLOverlay_Identifier GLOverlay::TRANSLATEF;
-const GLOverlay_Identifier GLOverlay::SCALEF;
-const GLOverlay_Identifier GLOverlay::PUSH_MATRIX;
-const GLOverlay_Identifier GLOverlay::POP_MATRIX;
-const GLOverlay_Identifier GLOverlay::COLOR3F;
-const GLOverlay_Identifier GLOverlay::COLOR4F;
-const GLOverlay_Identifier GLOverlay::POINTSIZE;
-const GLOverlay_Identifier GLOverlay::LINEWIDTH;
-const GLOverlay_Identifier GLOverlay::Identifier_MIN;
-const GLOverlay_Identifier GLOverlay::Identifier_MAX;
-const int GLOverlay::Identifier_ARRAYSIZE;
-#endif  // _MSC_VER
-#ifndef _MSC_VER
-const int GLOverlay::kHeaderFieldNumber;
-const int GLOverlay::kNameFieldNumber;
-const int GLOverlay::kCoordinateFrameTypeFieldNumber;
-const int GLOverlay::kOriginXFieldNumber;
-const int GLOverlay::kOriginYFieldNumber;
-const int GLOverlay::kOriginZFieldNumber;
-const int GLOverlay::kDataFieldNumber;
-#endif  // !_MSC_VER
-
-GLOverlay::GLOverlay()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void GLOverlay::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-GLOverlay::GLOverlay(const GLOverlay& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void GLOverlay::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  coordinateframetype_ = 0;
-  origin_x_ = 0;
-  origin_y_ = 0;
-  origin_z_ = 0;
-  data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-GLOverlay::~GLOverlay() {
-  SharedDtor();
-}
-
-void GLOverlay::SharedDtor() {
-  if (name_ != &::google::protobuf::internal::kEmptyString) {
-    delete name_;
-  }
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    delete data_;
-  }
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void GLOverlay::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* GLOverlay::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return GLOverlay_descriptor_;
-}
-
-const GLOverlay& GLOverlay::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-GLOverlay* GLOverlay::default_instance_ = NULL;
-
-GLOverlay* GLOverlay::New() const {
-  return new GLOverlay;
-}
-
-void GLOverlay::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-    if (has_name()) {
-      if (name_ != &::google::protobuf::internal::kEmptyString) {
-        name_->clear();
-      }
-    }
-    coordinateframetype_ = 0;
-    origin_x_ = 0;
-    origin_y_ = 0;
-    origin_z_ = 0;
-    if (has_data()) {
-      if (data_ != &::google::protobuf::internal::kEmptyString) {
-        data_->clear();
-      }
-    }
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool GLOverlay::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_name;
-        break;
-      }
-      
-      // optional string name = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_name:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadString(
-                input, this->mutable_name()));
-          ::google::protobuf::internal::WireFormat::VerifyUTF8String(
-            this->name().data(), this->name().length(),
-            ::google::protobuf::internal::WireFormat::PARSE);
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(24)) goto parse_coordinateFrameType;
-        break;
-      }
-      
-      // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_coordinateFrameType:
-          int value;
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   int, ::google::protobuf::internal::WireFormatLite::TYPE_ENUM>(
-                 input, &value)));
-          if (::px::GLOverlay_CoordinateFrameType_IsValid(value)) {
-            set_coordinateframetype(static_cast< ::px::GLOverlay_CoordinateFrameType >(value));
-          } else {
-            mutable_unknown_fields()->AddVarint(3, value);
-          }
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(33)) goto parse_origin_x;
-        break;
-      }
-      
-      // optional double origin_x = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_origin_x:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &origin_x_)));
-          set_has_origin_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(41)) goto parse_origin_y;
-        break;
-      }
-      
-      // optional double origin_y = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_origin_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &origin_y_)));
-          set_has_origin_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(49)) goto parse_origin_z;
-        break;
-      }
-      
-      // optional double origin_z = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_origin_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &origin_z_)));
-          set_has_origin_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(58)) goto parse_data;
-        break;
-      }
-      
-      // optional bytes data = 7;
-      case 7: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_data:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_data()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void GLOverlay::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // optional string name = 2;
-  if (has_name()) {
-    ::google::protobuf::internal::WireFormat::VerifyUTF8String(
-      this->name().data(), this->name().length(),
-      ::google::protobuf::internal::WireFormat::SERIALIZE);
-    ::google::protobuf::internal::WireFormatLite::WriteString(
-      2, this->name(), output);
-  }
-  
-  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-  if (has_coordinateframetype()) {
-    ::google::protobuf::internal::WireFormatLite::WriteEnum(
-      3, this->coordinateframetype(), output);
-  }
-  
-  // optional double origin_x = 4;
-  if (has_origin_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->origin_x(), output);
-  }
-  
-  // optional double origin_y = 5;
-  if (has_origin_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->origin_y(), output);
-  }
-  
-  // optional double origin_z = 6;
-  if (has_origin_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->origin_z(), output);
-  }
-  
-  // optional bytes data = 7;
-  if (has_data()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      7, this->data(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* GLOverlay::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // optional string name = 2;
-  if (has_name()) {
-    ::google::protobuf::internal::WireFormat::VerifyUTF8String(
-      this->name().data(), this->name().length(),
-      ::google::protobuf::internal::WireFormat::SERIALIZE);
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteStringToArray(
-        2, this->name(), target);
-  }
-  
-  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-  if (has_coordinateframetype()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteEnumToArray(
-      3, this->coordinateframetype(), target);
-  }
-  
-  // optional double origin_x = 4;
-  if (has_origin_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->origin_x(), target);
-  }
-  
-  // optional double origin_y = 5;
-  if (has_origin_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->origin_y(), target);
-  }
-  
-  // optional double origin_z = 6;
-  if (has_origin_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->origin_z(), target);
-  }
-  
-  // optional bytes data = 7;
-  if (has_data()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        7, this->data(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int GLOverlay::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-    // optional string name = 2;
-    if (has_name()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::StringSize(
-          this->name());
-    }
-    
-    // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-    if (has_coordinateframetype()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::EnumSize(this->coordinateframetype());
-    }
-    
-    // optional double origin_x = 4;
-    if (has_origin_x()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double origin_y = 5;
-    if (has_origin_y()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double origin_z = 6;
-    if (has_origin_z()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional bytes data = 7;
-    if (has_data()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->data());
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void GLOverlay::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const GLOverlay* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const GLOverlay*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void GLOverlay::MergeFrom(const GLOverlay& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-    if (from.has_name()) {
-      set_name(from.name());
-    }
-    if (from.has_coordinateframetype()) {
-      set_coordinateframetype(from.coordinateframetype());
-    }
-    if (from.has_origin_x()) {
-      set_origin_x(from.origin_x());
-    }
-    if (from.has_origin_y()) {
-      set_origin_y(from.origin_y());
-    }
-    if (from.has_origin_z()) {
-      set_origin_z(from.origin_z());
-    }
-    if (from.has_data()) {
-      set_data(from.data());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void GLOverlay::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void GLOverlay::CopyFrom(const GLOverlay& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool GLOverlay::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  return true;
-}
-
-void GLOverlay::Swap(GLOverlay* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    std::swap(name_, other->name_);
-    std::swap(coordinateframetype_, other->coordinateframetype_);
-    std::swap(origin_x_, other->origin_x_);
-    std::swap(origin_y_, other->origin_y_);
-    std::swap(origin_z_, other->origin_z_);
-    std::swap(data_, other->data_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata GLOverlay::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = GLOverlay_descriptor_;
-  metadata.reflection = GLOverlay_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int Obstacle::kXFieldNumber;
-const int Obstacle::kYFieldNumber;
-const int Obstacle::kZFieldNumber;
-const int Obstacle::kLengthFieldNumber;
-const int Obstacle::kWidthFieldNumber;
-const int Obstacle::kHeightFieldNumber;
-#endif  // !_MSC_VER
-
-Obstacle::Obstacle()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void Obstacle::InitAsDefaultInstance() {
-}
-
-Obstacle::Obstacle(const Obstacle& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void Obstacle::SharedCtor() {
-  _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  length_ = 0;
-  width_ = 0;
-  height_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-Obstacle::~Obstacle() {
-  SharedDtor();
-}
-
-void Obstacle::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void Obstacle::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* Obstacle::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return Obstacle_descriptor_;
-}
-
-const Obstacle& Obstacle::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-Obstacle* Obstacle::default_instance_ = NULL;
-
-Obstacle* Obstacle::New() const {
-  return new Obstacle;
-}
-
-void Obstacle::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    length_ = 0;
-    width_ = 0;
-    height_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool Obstacle::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // optional float x = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(21)) goto parse_y;
-        break;
-      }
-      
-      // optional float y = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_z;
-        break;
-      }
-      
-      // optional float z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(37)) goto parse_length;
-        break;
-      }
-      
-      // optional float length = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_length:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &length_)));
-          set_has_length();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(45)) goto parse_width;
-        break;
-      }
-      
-      // optional float width = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_width:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &width_)));
-          set_has_width();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(53)) goto parse_height;
-        break;
-      }
-      
-      // optional float height = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_height:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &height_)));
-          set_has_height();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void Obstacle::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // optional float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
-  }
-  
-  // optional float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
-  }
-  
-  // optional float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
-  }
-  
-  // optional float length = 4;
-  if (has_length()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->length(), output);
-  }
-  
-  // optional float width = 5;
-  if (has_width()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(5, this->width(), output);
-  }
-  
-  // optional float height = 6;
-  if (has_height()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(6, this->height(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* Obstacle::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // optional float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
-  }
-  
-  // optional float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
-  }
-  
-  // optional float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
-  }
-  
-  // optional float length = 4;
-  if (has_length()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->length(), target);
-  }
-  
-  // optional float width = 5;
-  if (has_width()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(5, this->width(), target);
-  }
-  
-  // optional float height = 6;
-  if (has_height()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(6, this->height(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int Obstacle::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // optional float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float length = 4;
-    if (has_length()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float width = 5;
-    if (has_width()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float height = 6;
-    if (has_height()) {
-      total_size += 1 + 4;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void Obstacle::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const Obstacle* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Obstacle*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void Obstacle::MergeFrom(const Obstacle& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_length()) {
-      set_length(from.length());
-    }
-    if (from.has_width()) {
-      set_width(from.width());
-    }
-    if (from.has_height()) {
-      set_height(from.height());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void Obstacle::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void Obstacle::CopyFrom(const Obstacle& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool Obstacle::IsInitialized() const {
-  
-  return true;
-}
-
-void Obstacle::Swap(Obstacle* other) {
-  if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(length_, other->length_);
-    std::swap(width_, other->width_);
-    std::swap(height_, other->height_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata Obstacle::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Obstacle_descriptor_;
-  metadata.reflection = Obstacle_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int ObstacleList::kHeaderFieldNumber;
-const int ObstacleList::kObstaclesFieldNumber;
-#endif  // !_MSC_VER
-
-ObstacleList::ObstacleList()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void ObstacleList::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-ObstacleList::ObstacleList(const ObstacleList& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void ObstacleList::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-ObstacleList::~ObstacleList() {
-  SharedDtor();
-}
-
-void ObstacleList::SharedDtor() {
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void ObstacleList::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* ObstacleList::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return ObstacleList_descriptor_;
-}
-
-const ObstacleList& ObstacleList::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-ObstacleList* ObstacleList::default_instance_ = NULL;
-
-ObstacleList* ObstacleList::New() const {
-  return new ObstacleList;
-}
-
-void ObstacleList::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-  }
-  obstacles_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool ObstacleList::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_obstacles;
-        break;
-      }
-      
-      // repeated .px.Obstacle obstacles = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_obstacles:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_obstacles()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_obstacles;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void ObstacleList::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // repeated .px.Obstacle obstacles = 2;
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->obstacles(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* ObstacleList::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // repeated .px.Obstacle obstacles = 2;
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->obstacles(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int ObstacleList::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-  }
-  // repeated .px.Obstacle obstacles = 2;
-  total_size += 1 * this->obstacles_size();
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->obstacles(i));
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void ObstacleList::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const ObstacleList* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const ObstacleList*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void ObstacleList::MergeFrom(const ObstacleList& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  obstacles_.MergeFrom(from.obstacles_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void ObstacleList::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void ObstacleList::CopyFrom(const ObstacleList& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool ObstacleList::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  return true;
-}
-
-void ObstacleList::Swap(ObstacleList* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    obstacles_.Swap(&other->obstacles_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata ObstacleList::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = ObstacleList_descriptor_;
-  metadata.reflection = ObstacleList_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int ObstacleMap::kHeaderFieldNumber;
-const int ObstacleMap::kTypeFieldNumber;
-const int ObstacleMap::kResolutionFieldNumber;
-const int ObstacleMap::kRowsFieldNumber;
-const int ObstacleMap::kColsFieldNumber;
-const int ObstacleMap::kMapR0FieldNumber;
-const int ObstacleMap::kMapC0FieldNumber;
-const int ObstacleMap::kArrayR0FieldNumber;
-const int ObstacleMap::kArrayC0FieldNumber;
-const int ObstacleMap::kDataFieldNumber;
-#endif  // !_MSC_VER
-
-ObstacleMap::ObstacleMap()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void ObstacleMap::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-ObstacleMap::ObstacleMap(const ObstacleMap& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void ObstacleMap::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  type_ = 0;
-  resolution_ = 0;
-  rows_ = 0;
-  cols_ = 0;
-  mapr0_ = 0;
-  mapc0_ = 0;
-  arrayr0_ = 0;
-  arrayc0_ = 0;
-  data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-ObstacleMap::~ObstacleMap() {
-  SharedDtor();
-}
-
-void ObstacleMap::SharedDtor() {
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    delete data_;
-  }
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void ObstacleMap::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* ObstacleMap::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return ObstacleMap_descriptor_;
-}
-
-const ObstacleMap& ObstacleMap::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-ObstacleMap* ObstacleMap::default_instance_ = NULL;
-
-ObstacleMap* ObstacleMap::New() const {
-  return new ObstacleMap;
-}
-
-void ObstacleMap::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-    type_ = 0;
-    resolution_ = 0;
-    rows_ = 0;
-    cols_ = 0;
-    mapr0_ = 0;
-    mapc0_ = 0;
-    arrayr0_ = 0;
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    arrayc0_ = 0;
-    if (has_data()) {
-      if (data_ != &::google::protobuf::internal::kEmptyString) {
-        data_->clear();
-      }
-    }
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool ObstacleMap::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(16)) goto parse_type;
-        break;
-      }
-      
-      // required int32 type = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &type_)));
-          set_has_type();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_resolution;
-        break;
-      }
-      
-      // optional float resolution = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_resolution:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &resolution_)));
-          set_has_resolution();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(32)) goto parse_rows;
-        break;
-      }
-      
-      // optional int32 rows = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_rows:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &rows_)));
-          set_has_rows();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(40)) goto parse_cols;
-        break;
-      }
-      
-      // optional int32 cols = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_cols:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &cols_)));
-          set_has_cols();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(48)) goto parse_mapR0;
-        break;
-      }
-      
-      // optional int32 mapR0 = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_mapR0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &mapr0_)));
-          set_has_mapr0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(56)) goto parse_mapC0;
-        break;
-      }
-      
-      // optional int32 mapC0 = 7;
-      case 7: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_mapC0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &mapc0_)));
-          set_has_mapc0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(64)) goto parse_arrayR0;
-        break;
-      }
-      
-      // optional int32 arrayR0 = 8;
-      case 8: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_arrayR0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &arrayr0_)));
-          set_has_arrayr0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(72)) goto parse_arrayC0;
-        break;
-      }
-      
-      // optional int32 arrayC0 = 9;
-      case 9: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_arrayC0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &arrayc0_)));
-          set_has_arrayc0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(82)) goto parse_data;
-        break;
-      }
-      
-      // optional bytes data = 10;
-      case 10: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_data:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_data()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void ObstacleMap::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // required int32 type = 2;
-  if (has_type()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->type(), output);
-  }
-  
-  // optional float resolution = 3;
-  if (has_resolution()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->resolution(), output);
-  }
-  
-  // optional int32 rows = 4;
-  if (has_rows()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->rows(), output);
-  }
-  
-  // optional int32 cols = 5;
-  if (has_cols()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(5, this->cols(), output);
-  }
-  
-  // optional int32 mapR0 = 6;
-  if (has_mapr0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(6, this->mapr0(), output);
-  }
-  
-  // optional int32 mapC0 = 7;
-  if (has_mapc0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(7, this->mapc0(), output);
-  }
-  
-  // optional int32 arrayR0 = 8;
-  if (has_arrayr0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(8, this->arrayr0(), output);
-  }
-  
-  // optional int32 arrayC0 = 9;
-  if (has_arrayc0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(9, this->arrayc0(), output);
-  }
-  
-  // optional bytes data = 10;
-  if (has_data()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      10, this->data(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* ObstacleMap::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // required int32 type = 2;
-  if (has_type()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->type(), target);
-  }
-  
-  // optional float resolution = 3;
-  if (has_resolution()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->resolution(), target);
-  }
-  
-  // optional int32 rows = 4;
-  if (has_rows()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->rows(), target);
-  }
-  
-  // optional int32 cols = 5;
-  if (has_cols()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(5, this->cols(), target);
-  }
-  
-  // optional int32 mapR0 = 6;
-  if (has_mapr0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(6, this->mapr0(), target);
-  }
-  
-  // optional int32 mapC0 = 7;
-  if (has_mapc0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(7, this->mapc0(), target);
-  }
-  
-  // optional int32 arrayR0 = 8;
-  if (has_arrayr0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(8, this->arrayr0(), target);
-  }
-  
-  // optional int32 arrayC0 = 9;
-  if (has_arrayc0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(9, this->arrayc0(), target);
-  }
-  
-  // optional bytes data = 10;
-  if (has_data()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        10, this->data(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int ObstacleMap::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-    // required int32 type = 2;
-    if (has_type()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->type());
-    }
-    
-    // optional float resolution = 3;
-    if (has_resolution()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional int32 rows = 4;
-    if (has_rows()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->rows());
-    }
-    
-    // optional int32 cols = 5;
-    if (has_cols()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->cols());
-    }
-    
-    // optional int32 mapR0 = 6;
-    if (has_mapr0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->mapr0());
-    }
-    
-    // optional int32 mapC0 = 7;
-    if (has_mapc0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->mapc0());
-    }
-    
-    // optional int32 arrayR0 = 8;
-    if (has_arrayr0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->arrayr0());
-    }
-    
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    // optional int32 arrayC0 = 9;
-    if (has_arrayc0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->arrayc0());
-    }
-    
-    // optional bytes data = 10;
-    if (has_data()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->data());
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void ObstacleMap::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const ObstacleMap* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const ObstacleMap*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void ObstacleMap::MergeFrom(const ObstacleMap& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-    if (from.has_type()) {
-      set_type(from.type());
-    }
-    if (from.has_resolution()) {
-      set_resolution(from.resolution());
-    }
-    if (from.has_rows()) {
-      set_rows(from.rows());
-    }
-    if (from.has_cols()) {
-      set_cols(from.cols());
-    }
-    if (from.has_mapr0()) {
-      set_mapr0(from.mapr0());
-    }
-    if (from.has_mapc0()) {
-      set_mapc0(from.mapc0());
-    }
-    if (from.has_arrayr0()) {
-      set_arrayr0(from.arrayr0());
-    }
-  }
-  if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (from.has_arrayc0()) {
-      set_arrayc0(from.arrayc0());
-    }
-    if (from.has_data()) {
-      set_data(from.data());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void ObstacleMap::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void ObstacleMap::CopyFrom(const ObstacleMap& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool ObstacleMap::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  return true;
-}
-
-void ObstacleMap::Swap(ObstacleMap* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    std::swap(type_, other->type_);
-    std::swap(resolution_, other->resolution_);
-    std::swap(rows_, other->rows_);
-    std::swap(cols_, other->cols_);
-    std::swap(mapr0_, other->mapr0_);
-    std::swap(mapc0_, other->mapc0_);
-    std::swap(arrayr0_, other->arrayr0_);
-    std::swap(arrayc0_, other->arrayc0_);
-    std::swap(data_, other->data_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata ObstacleMap::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = ObstacleMap_descriptor_;
-  metadata.reflection = ObstacleMap_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int Path::kHeaderFieldNumber;
-const int Path::kWaypointsFieldNumber;
-#endif  // !_MSC_VER
-
-Path::Path()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void Path::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-Path::Path(const Path& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void Path::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-Path::~Path() {
-  SharedDtor();
-}
-
-void Path::SharedDtor() {
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void Path::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* Path::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return Path_descriptor_;
-}
-
-const Path& Path::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-Path* Path::default_instance_ = NULL;
-
-Path* Path::New() const {
-  return new Path;
-}
-
-void Path::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-  }
-  waypoints_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool Path::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_waypoints;
-        break;
-      }
-      
-      // repeated .px.Waypoint waypoints = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_waypoints:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_waypoints()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_waypoints;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void Path::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // repeated .px.Waypoint waypoints = 2;
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->waypoints(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* Path::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // repeated .px.Waypoint waypoints = 2;
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->waypoints(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int Path::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-  }
-  // repeated .px.Waypoint waypoints = 2;
-  total_size += 1 * this->waypoints_size();
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->waypoints(i));
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void Path::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const Path* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Path*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void Path::MergeFrom(const Path& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  waypoints_.MergeFrom(from.waypoints_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void Path::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void Path::CopyFrom(const Path& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool Path::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  for (int i = 0; i < waypoints_size(); i++) {
-    if (!this->waypoints(i).IsInitialized()) return false;
-  }
-  return true;
-}
-
-void Path::Swap(Path* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    waypoints_.Swap(&other->waypoints_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata Path::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Path_descriptor_;
-  metadata.reflection = Path_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int PointCloudXYZI_PointXYZI::kXFieldNumber;
-const int PointCloudXYZI_PointXYZI::kYFieldNumber;
-const int PointCloudXYZI_PointXYZI::kZFieldNumber;
-const int PointCloudXYZI_PointXYZI::kIntensityFieldNumber;
-#endif  // !_MSC_VER
-
-PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void PointCloudXYZI_PointXYZI::InitAsDefaultInstance() {
-}
-
-PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI_PointXYZI::SharedCtor() {
-  _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  intensity_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-PointCloudXYZI_PointXYZI::~PointCloudXYZI_PointXYZI() {
-  SharedDtor();
-}
-
-void PointCloudXYZI_PointXYZI::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void PointCloudXYZI_PointXYZI::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZI_PointXYZI_descriptor_;
-}
-
-const PointCloudXYZI_PointXYZI& PointCloudXYZI_PointXYZI::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::default_instance_ = NULL;
-
-PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::New() const {
-  return new PointCloudXYZI_PointXYZI;
-}
-
-void PointCloudXYZI_PointXYZI::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    intensity_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool PointCloudXYZI_PointXYZI::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required float x = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(21)) goto parse_y;
-        break;
-      }
-      
-      // required float y = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_z;
-        break;
-      }
-      
-      // required float z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(37)) goto parse_intensity;
-        break;
-      }
-      
-      // required float intensity = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_intensity:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &intensity_)));
-          set_has_intensity();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void PointCloudXYZI_PointXYZI::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
-  }
-  
-  // required float intensity = 4;
-  if (has_intensity()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->intensity(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* PointCloudXYZI_PointXYZI::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
-  }
-  
-  // required float intensity = 4;
-  if (has_intensity()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->intensity(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int PointCloudXYZI_PointXYZI::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float intensity = 4;
-    if (has_intensity()) {
-      total_size += 1 + 4;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void PointCloudXYZI_PointXYZI::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZI_PointXYZI* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZI_PointXYZI*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void PointCloudXYZI_PointXYZI::MergeFrom(const PointCloudXYZI_PointXYZI& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_intensity()) {
-      set_intensity(from.intensity());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZI_PointXYZI::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI_PointXYZI::CopyFrom(const PointCloudXYZI_PointXYZI& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool PointCloudXYZI_PointXYZI::IsInitialized() const {
-  if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false;
-  
-  return true;
-}
-
-void PointCloudXYZI_PointXYZI::Swap(PointCloudXYZI_PointXYZI* other) {
-  if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(intensity_, other->intensity_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata PointCloudXYZI_PointXYZI::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZI_PointXYZI_descriptor_;
-  metadata.reflection = PointCloudXYZI_PointXYZI_reflection_;
-  return metadata;
-}
-
-
-// -------------------------------------------------------------------
-
-#ifndef _MSC_VER
-const int PointCloudXYZI::kHeaderFieldNumber;
-const int PointCloudXYZI::kPointsFieldNumber;
-#endif  // !_MSC_VER
-
-PointCloudXYZI::PointCloudXYZI()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void PointCloudXYZI::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-PointCloudXYZI::PointCloudXYZI(const PointCloudXYZI& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-PointCloudXYZI::~PointCloudXYZI() {
-  SharedDtor();
-}
-
-void PointCloudXYZI::SharedDtor() {
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void PointCloudXYZI::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* PointCloudXYZI::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZI_descriptor_;
-}
-
-const PointCloudXYZI& PointCloudXYZI::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-PointCloudXYZI* PointCloudXYZI::default_instance_ = NULL;
-
-PointCloudXYZI* PointCloudXYZI::New() const {
-  return new PointCloudXYZI;
-}
-
-void PointCloudXYZI::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-  }
-  points_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool PointCloudXYZI::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_points;
-        break;
-      }
-      
-      // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_points:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_points()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_points;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void PointCloudXYZI::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->points(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* PointCloudXYZI::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->points(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int PointCloudXYZI::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-  }
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  total_size += 1 * this->points_size();
-  for (int i = 0; i < this->points_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->points(i));
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void PointCloudXYZI::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZI* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZI*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void PointCloudXYZI::MergeFrom(const PointCloudXYZI& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  points_.MergeFrom(from.points_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZI::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI::CopyFrom(const PointCloudXYZI& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool PointCloudXYZI::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  for (int i = 0; i < points_size(); i++) {
-    if (!this->points(i).IsInitialized()) return false;
-  }
-  return true;
-}
-
-void PointCloudXYZI::Swap(PointCloudXYZI* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    points_.Swap(&other->points_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata PointCloudXYZI::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZI_descriptor_;
-  metadata.reflection = PointCloudXYZI_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int PointCloudXYZRGB_PointXYZRGB::kXFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kYFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kZFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kRgbFieldNumber;
-#endif  // !_MSC_VER
-
-PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void PointCloudXYZRGB_PointXYZRGB::InitAsDefaultInstance() {
-}
-
-PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void PointCloudXYZRGB_PointXYZRGB::SharedCtor() {
-  _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  rgb_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-PointCloudXYZRGB_PointXYZRGB::~PointCloudXYZRGB_PointXYZRGB() {
-  SharedDtor();
-}
-
-void PointCloudXYZRGB_PointXYZRGB::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void PointCloudXYZRGB_PointXYZRGB::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZRGB_PointXYZRGB_descriptor_;
-}
-
-const PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB_PointXYZRGB::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::default_instance_ = NULL;
-
-PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::New() const {
-  return new PointCloudXYZRGB_PointXYZRGB;
-}
-
-void PointCloudXYZRGB_PointXYZRGB::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    rgb_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool PointCloudXYZRGB_PointXYZRGB::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required float x = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(21)) goto parse_y;
-        break;
-      }
-      
-      // required float y = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_z;
-        break;
-      }
-      
-      // required float z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(37)) goto parse_rgb;
-        break;
-      }
-      
-      // required float rgb = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_rgb:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &rgb_)));
-          set_has_rgb();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
-  }
-  
-  // required float rgb = 4;
-  if (has_rgb()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->rgb(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
-  }
-  
-  // required float rgb = 4;
-  if (has_rgb()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->rgb(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int PointCloudXYZRGB_PointXYZRGB::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float rgb = 4;
-    if (has_rgb()) {
-      total_size += 1 + 4;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZRGB_PointXYZRGB* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZRGB_PointXYZRGB*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_rgb()) {
-      set_rgb(from.rgb());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool PointCloudXYZRGB_PointXYZRGB::IsInitialized() const {
-  if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false;
-  
-  return true;
-}
-
-void PointCloudXYZRGB_PointXYZRGB::Swap(PointCloudXYZRGB_PointXYZRGB* other) {
-  if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(rgb_, other->rgb_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata PointCloudXYZRGB_PointXYZRGB::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZRGB_PointXYZRGB_descriptor_;
-  metadata.reflection = PointCloudXYZRGB_PointXYZRGB_reflection_;
-  return metadata;
-}
-
-
-// -------------------------------------------------------------------
-
-#ifndef _MSC_VER
-const int PointCloudXYZRGB::kHeaderFieldNumber;
-const int PointCloudXYZRGB::kPointsFieldNumber;
-#endif  // !_MSC_VER
-
-PointCloudXYZRGB::PointCloudXYZRGB()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void PointCloudXYZRGB::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-PointCloudXYZRGB::PointCloudXYZRGB(const PointCloudXYZRGB& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void PointCloudXYZRGB::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-PointCloudXYZRGB::~PointCloudXYZRGB() {
-  SharedDtor();
-}
-
-void PointCloudXYZRGB::SharedDtor() {
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void PointCloudXYZRGB::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* PointCloudXYZRGB::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZRGB_descriptor_;
-}
-
-const PointCloudXYZRGB& PointCloudXYZRGB::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-PointCloudXYZRGB* PointCloudXYZRGB::default_instance_ = NULL;
-
-PointCloudXYZRGB* PointCloudXYZRGB::New() const {
-  return new PointCloudXYZRGB;
-}
-
-void PointCloudXYZRGB::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-  }
-  points_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool PointCloudXYZRGB::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_points;
-        break;
-      }
-      
-      // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_points:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_points()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_points;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void PointCloudXYZRGB::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->points(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* PointCloudXYZRGB::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->points(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int PointCloudXYZRGB::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-  }
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  total_size += 1 * this->points_size();
-  for (int i = 0; i < this->points_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->points(i));
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void PointCloudXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZRGB* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZRGB*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void PointCloudXYZRGB::MergeFrom(const PointCloudXYZRGB& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  points_.MergeFrom(from.points_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZRGB::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZRGB::CopyFrom(const PointCloudXYZRGB& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool PointCloudXYZRGB::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  for (int i = 0; i < points_size(); i++) {
-    if (!this->points(i).IsInitialized()) return false;
-  }
-  return true;
-}
-
-void PointCloudXYZRGB::Swap(PointCloudXYZRGB* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    points_.Swap(&other->points_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata PointCloudXYZRGB::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZRGB_descriptor_;
-  metadata.reflection = PointCloudXYZRGB_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int RGBDImage::kHeaderFieldNumber;
-const int RGBDImage::kColsFieldNumber;
-const int RGBDImage::kRowsFieldNumber;
-const int RGBDImage::kStep1FieldNumber;
-const int RGBDImage::kType1FieldNumber;
-const int RGBDImage::kImageData1FieldNumber;
-const int RGBDImage::kStep2FieldNumber;
-const int RGBDImage::kType2FieldNumber;
-const int RGBDImage::kImageData2FieldNumber;
-const int RGBDImage::kCameraConfigFieldNumber;
-const int RGBDImage::kCameraTypeFieldNumber;
-const int RGBDImage::kRollFieldNumber;
-const int RGBDImage::kPitchFieldNumber;
-const int RGBDImage::kYawFieldNumber;
-const int RGBDImage::kLonFieldNumber;
-const int RGBDImage::kLatFieldNumber;
-const int RGBDImage::kAltFieldNumber;
-const int RGBDImage::kGroundXFieldNumber;
-const int RGBDImage::kGroundYFieldNumber;
-const int RGBDImage::kGroundZFieldNumber;
-const int RGBDImage::kCameraMatrixFieldNumber;
-#endif  // !_MSC_VER
-
-RGBDImage::RGBDImage()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void RGBDImage::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-RGBDImage::RGBDImage(const RGBDImage& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void RGBDImage::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  cols_ = 0u;
-  rows_ = 0u;
-  step1_ = 0u;
-  type1_ = 0u;
-  imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  step2_ = 0u;
-  type2_ = 0u;
-  imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  camera_config_ = 0u;
-  camera_type_ = 0u;
-  roll_ = 0;
-  pitch_ = 0;
-  yaw_ = 0;
-  lon_ = 0;
-  lat_ = 0;
-  alt_ = 0;
-  ground_x_ = 0;
-  ground_y_ = 0;
-  ground_z_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-RGBDImage::~RGBDImage() {
-  SharedDtor();
-}
-
-void RGBDImage::SharedDtor() {
-  if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-    delete imagedata1_;
-  }
-  if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-    delete imagedata2_;
-  }
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void RGBDImage::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* RGBDImage::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return RGBDImage_descriptor_;
-}
-
-const RGBDImage& RGBDImage::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-RGBDImage* RGBDImage::default_instance_ = NULL;
-
-RGBDImage* RGBDImage::New() const {
-  return new RGBDImage;
-}
-
-void RGBDImage::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-    cols_ = 0u;
-    rows_ = 0u;
-    step1_ = 0u;
-    type1_ = 0u;
-    if (has_imagedata1()) {
-      if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-        imagedata1_->clear();
-      }
-    }
-    step2_ = 0u;
-    type2_ = 0u;
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (has_imagedata2()) {
-      if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-        imagedata2_->clear();
-      }
-    }
-    camera_config_ = 0u;
-    camera_type_ = 0u;
-    roll_ = 0;
-    pitch_ = 0;
-    yaw_ = 0;
-    lon_ = 0;
-    lat_ = 0;
-  }
-  if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    alt_ = 0;
-    ground_x_ = 0;
-    ground_y_ = 0;
-    ground_z_ = 0;
-  }
-  camera_matrix_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool RGBDImage::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(16)) goto parse_cols;
-        break;
-      }
-      
-      // required uint32 cols = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_cols:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &cols_)));
-          set_has_cols();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(24)) goto parse_rows;
-        break;
-      }
-      
-      // required uint32 rows = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_rows:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &rows_)));
-          set_has_rows();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(32)) goto parse_step1;
-        break;
-      }
-      
-      // required uint32 step1 = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_step1:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &step1_)));
-          set_has_step1();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(40)) goto parse_type1;
-        break;
-      }
-      
-      // required uint32 type1 = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type1:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &type1_)));
-          set_has_type1();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(50)) goto parse_imageData1;
-        break;
-      }
-      
-      // required bytes imageData1 = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_imageData1:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_imagedata1()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(56)) goto parse_step2;
-        break;
-      }
-      
-      // required uint32 step2 = 7;
-      case 7: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_step2:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &step2_)));
-          set_has_step2();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(64)) goto parse_type2;
-        break;
-      }
-      
-      // required uint32 type2 = 8;
-      case 8: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type2:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &type2_)));
-          set_has_type2();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(74)) goto parse_imageData2;
-        break;
-      }
-      
-      // required bytes imageData2 = 9;
-      case 9: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_imageData2:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_imagedata2()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(80)) goto parse_camera_config;
-        break;
-      }
-      
-      // optional uint32 camera_config = 10;
-      case 10: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_camera_config:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &camera_config_)));
-          set_has_camera_config();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(88)) goto parse_camera_type;
-        break;
-      }
-      
-      // optional uint32 camera_type = 11;
-      case 11: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_camera_type:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &camera_type_)));
-          set_has_camera_type();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(101)) goto parse_roll;
-        break;
-      }
-      
-      // optional float roll = 12;
-      case 12: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_roll:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &roll_)));
-          set_has_roll();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(109)) goto parse_pitch;
-        break;
-      }
-      
-      // optional float pitch = 13;
-      case 13: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_pitch:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &pitch_)));
-          set_has_pitch();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(117)) goto parse_yaw;
-        break;
-      }
-      
-      // optional float yaw = 14;
-      case 14: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_yaw:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &yaw_)));
-          set_has_yaw();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(125)) goto parse_lon;
-        break;
-      }
-      
-      // optional float lon = 15;
-      case 15: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_lon:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &lon_)));
-          set_has_lon();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(133)) goto parse_lat;
-        break;
-      }
-      
-      // optional float lat = 16;
-      case 16: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_lat:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &lat_)));
-          set_has_lat();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(141)) goto parse_alt;
-        break;
-      }
-      
-      // optional float alt = 17;
-      case 17: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_alt:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &alt_)));
-          set_has_alt();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(149)) goto parse_ground_x;
-        break;
-      }
-      
-      // optional float ground_x = 18;
-      case 18: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_x:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_x_)));
-          set_has_ground_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(157)) goto parse_ground_y;
-        break;
-      }
-      
-      // optional float ground_y = 19;
-      case 19: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_y_)));
-          set_has_ground_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(165)) goto parse_ground_z;
-        break;
-      }
-      
-      // optional float ground_z = 20;
-      case 20: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_z_)));
-          set_has_ground_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(173)) goto parse_camera_matrix;
-        break;
-      }
-      
-      // repeated float camera_matrix = 21;
-      case 21: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_camera_matrix:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadRepeatedPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 2, 173, input, this->mutable_camera_matrix())));
-        } else if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag)
-                   == ::google::protobuf::internal::WireFormatLite::
-                      WIRETYPE_LENGTH_DELIMITED) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPackedPrimitiveNoInline<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, this->mutable_camera_matrix())));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(173)) goto parse_camera_matrix;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void RGBDImage::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // required uint32 cols = 2;
-  if (has_cols()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(2, this->cols(), output);
-  }
-  
-  // required uint32 rows = 3;
-  if (has_rows()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(3, this->rows(), output);
-  }
-  
-  // required uint32 step1 = 4;
-  if (has_step1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(4, this->step1(), output);
-  }
-  
-  // required uint32 type1 = 5;
-  if (has_type1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(5, this->type1(), output);
-  }
-  
-  // required bytes imageData1 = 6;
-  if (has_imagedata1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      6, this->imagedata1(), output);
-  }
-  
-  // required uint32 step2 = 7;
-  if (has_step2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(7, this->step2(), output);
-  }
-  
-  // required uint32 type2 = 8;
-  if (has_type2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(8, this->type2(), output);
-  }
-  
-  // required bytes imageData2 = 9;
-  if (has_imagedata2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      9, this->imagedata2(), output);
-  }
-  
-  // optional uint32 camera_config = 10;
-  if (has_camera_config()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(10, this->camera_config(), output);
-  }
-  
-  // optional uint32 camera_type = 11;
-  if (has_camera_type()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(11, this->camera_type(), output);
-  }
-  
-  // optional float roll = 12;
-  if (has_roll()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(12, this->roll(), output);
-  }
-  
-  // optional float pitch = 13;
-  if (has_pitch()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(13, this->pitch(), output);
-  }
-  
-  // optional float yaw = 14;
-  if (has_yaw()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(14, this->yaw(), output);
-  }
-  
-  // optional float lon = 15;
-  if (has_lon()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(15, this->lon(), output);
-  }
-  
-  // optional float lat = 16;
-  if (has_lat()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(16, this->lat(), output);
-  }
-  
-  // optional float alt = 17;
-  if (has_alt()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(17, this->alt(), output);
-  }
-  
-  // optional float ground_x = 18;
-  if (has_ground_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(18, this->ground_x(), output);
-  }
-  
-  // optional float ground_y = 19;
-  if (has_ground_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(19, this->ground_y(), output);
-  }
-  
-  // optional float ground_z = 20;
-  if (has_ground_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(20, this->ground_z(), output);
-  }
-  
-  // repeated float camera_matrix = 21;
-  for (int i = 0; i < this->camera_matrix_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(
-      21, this->camera_matrix(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* RGBDImage::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // required uint32 cols = 2;
-  if (has_cols()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(2, this->cols(), target);
-  }
-  
-  // required uint32 rows = 3;
-  if (has_rows()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(3, this->rows(), target);
-  }
-  
-  // required uint32 step1 = 4;
-  if (has_step1()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(4, this->step1(), target);
-  }
-  
-  // required uint32 type1 = 5;
-  if (has_type1()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(5, this->type1(), target);
-  }
-  
-  // required bytes imageData1 = 6;
-  if (has_imagedata1()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        6, this->imagedata1(), target);
-  }
-  
-  // required uint32 step2 = 7;
-  if (has_step2()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(7, this->step2(), target);
-  }
-  
-  // required uint32 type2 = 8;
-  if (has_type2()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(8, this->type2(), target);
-  }
-  
-  // required bytes imageData2 = 9;
-  if (has_imagedata2()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        9, this->imagedata2(), target);
-  }
-  
-  // optional uint32 camera_config = 10;
-  if (has_camera_config()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(10, this->camera_config(), target);
-  }
-  
-  // optional uint32 camera_type = 11;
-  if (has_camera_type()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(11, this->camera_type(), target);
-  }
-  
-  // optional float roll = 12;
-  if (has_roll()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(12, this->roll(), target);
-  }
-  
-  // optional float pitch = 13;
-  if (has_pitch()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(13, this->pitch(), target);
-  }
-  
-  // optional float yaw = 14;
-  if (has_yaw()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(14, this->yaw(), target);
-  }
-  
-  // optional float lon = 15;
-  if (has_lon()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(15, this->lon(), target);
-  }
-  
-  // optional float lat = 16;
-  if (has_lat()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(16, this->lat(), target);
-  }
-  
-  // optional float alt = 17;
-  if (has_alt()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(17, this->alt(), target);
-  }
-  
-  // optional float ground_x = 18;
-  if (has_ground_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(18, this->ground_x(), target);
-  }
-  
-  // optional float ground_y = 19;
-  if (has_ground_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(19, this->ground_y(), target);
-  }
-  
-  // optional float ground_z = 20;
-  if (has_ground_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(20, this->ground_z(), target);
-  }
-  
-  // repeated float camera_matrix = 21;
-  for (int i = 0; i < this->camera_matrix_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteFloatToArray(21, this->camera_matrix(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int RGBDImage::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-    // required uint32 cols = 2;
-    if (has_cols()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->cols());
-    }
-    
-    // required uint32 rows = 3;
-    if (has_rows()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->rows());
-    }
-    
-    // required uint32 step1 = 4;
-    if (has_step1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->step1());
-    }
-    
-    // required uint32 type1 = 5;
-    if (has_type1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->type1());
-    }
-    
-    // required bytes imageData1 = 6;
-    if (has_imagedata1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->imagedata1());
-    }
-    
-    // required uint32 step2 = 7;
-    if (has_step2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->step2());
-    }
-    
-    // required uint32 type2 = 8;
-    if (has_type2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->type2());
-    }
-    
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    // required bytes imageData2 = 9;
-    if (has_imagedata2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->imagedata2());
-    }
-    
-    // optional uint32 camera_config = 10;
-    if (has_camera_config()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->camera_config());
-    }
-    
-    // optional uint32 camera_type = 11;
-    if (has_camera_type()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->camera_type());
-    }
-    
-    // optional float roll = 12;
-    if (has_roll()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float pitch = 13;
-    if (has_pitch()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float yaw = 14;
-    if (has_yaw()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float lon = 15;
-    if (has_lon()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float lat = 16;
-    if (has_lat()) {
-      total_size += 2 + 4;
-    }
-    
-  }
-  if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    // optional float alt = 17;
-    if (has_alt()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_x = 18;
-    if (has_ground_x()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_y = 19;
-    if (has_ground_y()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_z = 20;
-    if (has_ground_z()) {
-      total_size += 2 + 4;
-    }
-    
-  }
-  // repeated float camera_matrix = 21;
-  {
-    int data_size = 0;
-    data_size = 4 * this->camera_matrix_size();
-    total_size += 2 * this->camera_matrix_size() + data_size;
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void RGBDImage::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const RGBDImage* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const RGBDImage*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void RGBDImage::MergeFrom(const RGBDImage& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  camera_matrix_.MergeFrom(from.camera_matrix_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-    if (from.has_cols()) {
-      set_cols(from.cols());
-    }
-    if (from.has_rows()) {
-      set_rows(from.rows());
-    }
-    if (from.has_step1()) {
-      set_step1(from.step1());
-    }
-    if (from.has_type1()) {
-      set_type1(from.type1());
-    }
-    if (from.has_imagedata1()) {
-      set_imagedata1(from.imagedata1());
-    }
-    if (from.has_step2()) {
-      set_step2(from.step2());
-    }
-    if (from.has_type2()) {
-      set_type2(from.type2());
-    }
-  }
-  if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (from.has_imagedata2()) {
-      set_imagedata2(from.imagedata2());
-    }
-    if (from.has_camera_config()) {
-      set_camera_config(from.camera_config());
-    }
-    if (from.has_camera_type()) {
-      set_camera_type(from.camera_type());
-    }
-    if (from.has_roll()) {
-      set_roll(from.roll());
-    }
-    if (from.has_pitch()) {
-      set_pitch(from.pitch());
-    }
-    if (from.has_yaw()) {
-      set_yaw(from.yaw());
-    }
-    if (from.has_lon()) {
-      set_lon(from.lon());
-    }
-    if (from.has_lat()) {
-      set_lat(from.lat());
-    }
-  }
-  if (from._has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    if (from.has_alt()) {
-      set_alt(from.alt());
-    }
-    if (from.has_ground_x()) {
-      set_ground_x(from.ground_x());
-    }
-    if (from.has_ground_y()) {
-      set_ground_y(from.ground_y());
-    }
-    if (from.has_ground_z()) {
-      set_ground_z(from.ground_z());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void RGBDImage::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void RGBDImage::CopyFrom(const RGBDImage& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool RGBDImage::IsInitialized() const {
-  if ((_has_bits_[0] & 0x000001ff) != 0x000001ff) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  return true;
-}
-
-void RGBDImage::Swap(RGBDImage* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    std::swap(cols_, other->cols_);
-    std::swap(rows_, other->rows_);
-    std::swap(step1_, other->step1_);
-    std::swap(type1_, other->type1_);
-    std::swap(imagedata1_, other->imagedata1_);
-    std::swap(step2_, other->step2_);
-    std::swap(type2_, other->type2_);
-    std::swap(imagedata2_, other->imagedata2_);
-    std::swap(camera_config_, other->camera_config_);
-    std::swap(camera_type_, other->camera_type_);
-    std::swap(roll_, other->roll_);
-    std::swap(pitch_, other->pitch_);
-    std::swap(yaw_, other->yaw_);
-    std::swap(lon_, other->lon_);
-    std::swap(lat_, other->lat_);
-    std::swap(alt_, other->alt_);
-    std::swap(ground_x_, other->ground_x_);
-    std::swap(ground_y_, other->ground_y_);
-    std::swap(ground_z_, other->ground_z_);
-    camera_matrix_.Swap(&other->camera_matrix_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata RGBDImage::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = RGBDImage_descriptor_;
-  metadata.reflection = RGBDImage_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int Waypoint::kXFieldNumber;
-const int Waypoint::kYFieldNumber;
-const int Waypoint::kZFieldNumber;
-const int Waypoint::kRollFieldNumber;
-const int Waypoint::kPitchFieldNumber;
-const int Waypoint::kYawFieldNumber;
-#endif  // !_MSC_VER
-
-Waypoint::Waypoint()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void Waypoint::InitAsDefaultInstance() {
-}
-
-Waypoint::Waypoint(const Waypoint& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void Waypoint::SharedCtor() {
-  _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  roll_ = 0;
-  pitch_ = 0;
-  yaw_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-Waypoint::~Waypoint() {
-  SharedDtor();
-}
-
-void Waypoint::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void Waypoint::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* Waypoint::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return Waypoint_descriptor_;
-}
-
-const Waypoint& Waypoint::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-Waypoint* Waypoint::default_instance_ = NULL;
-
-Waypoint* Waypoint::New() const {
-  return new Waypoint;
-}
-
-void Waypoint::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    roll_ = 0;
-    pitch_ = 0;
-    yaw_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool Waypoint::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required double x = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &x_)));
-          set_has_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(17)) goto parse_y;
-        break;
-      }
-      
-      // required double y = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(25)) goto parse_z;
-        break;
-      }
-      
-      // optional double z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(33)) goto parse_roll;
-        break;
-      }
-      
-      // optional double roll = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_roll:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &roll_)));
-          set_has_roll();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(41)) goto parse_pitch;
-        break;
-      }
-      
-      // optional double pitch = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_pitch:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &pitch_)));
-          set_has_pitch();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(49)) goto parse_yaw;
-        break;
-      }
-      
-      // optional double yaw = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_yaw:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &yaw_)));
-          set_has_yaw();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void Waypoint::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required double x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(1, this->x(), output);
-  }
-  
-  // required double y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(2, this->y(), output);
-  }
-  
-  // optional double z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->z(), output);
-  }
-  
-  // optional double roll = 4;
-  if (has_roll()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->roll(), output);
-  }
-  
-  // optional double pitch = 5;
-  if (has_pitch()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->pitch(), output);
-  }
-  
-  // optional double yaw = 6;
-  if (has_yaw()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->yaw(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* Waypoint::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required double x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(1, this->x(), target);
-  }
-  
-  // required double y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(2, this->y(), target);
-  }
-  
-  // optional double z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->z(), target);
-  }
-  
-  // optional double roll = 4;
-  if (has_roll()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->roll(), target);
-  }
-  
-  // optional double pitch = 5;
-  if (has_pitch()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->pitch(), target);
-  }
-  
-  // optional double yaw = 6;
-  if (has_yaw()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->yaw(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int Waypoint::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required double x = 1;
-    if (has_x()) {
-      total_size += 1 + 8;
-    }
-    
-    // required double y = 2;
-    if (has_y()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double z = 3;
-    if (has_z()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double roll = 4;
-    if (has_roll()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double pitch = 5;
-    if (has_pitch()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double yaw = 6;
-    if (has_yaw()) {
-      total_size += 1 + 8;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void Waypoint::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const Waypoint* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Waypoint*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void Waypoint::MergeFrom(const Waypoint& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_roll()) {
-      set_roll(from.roll());
-    }
-    if (from.has_pitch()) {
-      set_pitch(from.pitch());
-    }
-    if (from.has_yaw()) {
-      set_yaw(from.yaw());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void Waypoint::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void Waypoint::CopyFrom(const Waypoint& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool Waypoint::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false;
-  
-  return true;
-}
-
-void Waypoint::Swap(Waypoint* other) {
-  if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(roll_, other->roll_);
-    std::swap(pitch_, other->pitch_);
-    std::swap(yaw_, other->yaw_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata Waypoint::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Waypoint_descriptor_;
-  metadata.reflection = Waypoint_reflection_;
-  return metadata;
-}
-
-
-// @@protoc_insertion_point(namespace_scope)
-
-}  // namespace px
-
-// @@protoc_insertion_point(global_scope)
diff --git a/mavlink/share/pyshared/pymavlink/.gitignore b/mavlink/share/pyshared/pymavlink/.gitignore
deleted file mode 100644
index 3d395ecda..000000000
--- a/mavlink/share/pyshared/pymavlink/.gitignore
+++ /dev/null
@@ -1,13 +0,0 @@
-apidocs/
-*.zip
-*.pyc
-send.sh
-generator/C/include/ardupilotmega
-generator/C/include/common
-generator/C/include/pixhawk
-generator/C/include/minimal
-generator/C/include/ualberta
-generator/C/include/slugs
-testmav0.9*
-testmav1.0*
-Debug/
diff --git a/mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde b/mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde
deleted file mode 100644
index b903b5c16..000000000
--- a/mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde
+++ /dev/null
@@ -1,55 +0,0 @@
-/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
-
-/*
-  send all possible mavlink messages
-  Andrew Tridgell July 2011
-*/
-
-// AVR runtime
-#include <avr/io.h>
-#include <avr/eeprom.h>
-#include <avr/pgmspace.h>
-#include <math.h>
-
-// Libraries
-#include <FastSerial.h>
-#include <AP_Common.h>
-#include <GCS_MAVLink.h>
-#include "mavtest.h"
-
-FastSerialPort0(Serial);        // FTDI/console
-FastSerialPort1(Serial1);       // GPS port
-FastSerialPort3(Serial3);       // Telemetry port
-
-#define SERIAL0_BAUD 115200
-#define SERIAL3_BAUD 115200
-
-void setup() {
-	Serial.begin(SERIAL0_BAUD, 128, 128);
-	Serial3.begin(SERIAL3_BAUD, 128, 128);
-	mavlink_comm_0_port = &Serial;
-	mavlink_comm_1_port = &Serial3;
-}
-
-
-
-void loop()
-{
-	Serial.println("Starting MAVLink test generator\n");
-	while (1) {
-		mavlink_msg_heartbeat_send(
-			MAVLINK_COMM_0,
-			mavlink_system.type,
-			MAV_AUTOPILOT_ARDUPILOTMEGA);
-
-		mavlink_msg_heartbeat_send(
-			MAVLINK_COMM_1,
-			mavlink_system.type,
-			MAV_AUTOPILOT_ARDUPILOTMEGA);
-
-		mavtest_generate_outputs(MAVLINK_COMM_0);
-		mavtest_generate_outputs(MAVLINK_COMM_1);
-		delay(500);
-	}
-}
-
diff --git a/mavlink/share/pyshared/pymavlink/README.txt b/mavlink/share/pyshared/pymavlink/README.txt
deleted file mode 100644
index 386013e11..000000000
--- a/mavlink/share/pyshared/pymavlink/README.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-This is a python implementation of the MAVLink protocol. 
-
-Please see http://www.qgroundcontrol.org/mavlink/pymavlink for
-documentation
diff --git a/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py b/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py
deleted file mode 100644
index d7d82256c..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py
+++ /dev/null
@@ -1,67 +0,0 @@
-#!/usr/bin/env python
-
-'''
-set stream rate on an APM 
-'''
-
-import sys, struct, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("apmsetrate.py [options]")
-
-parser.add_option("--baudrate", dest="baudrate", type='int',
-                  help="master port baud rate", default=115200)
-parser.add_option("--device", dest="device", default=None, help="serial device")
-parser.add_option("--rate", dest="rate", default=4, type='int', help="requested stream rate")
-parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int',
-                  default=255, help='MAVLink source system for this GCS')
-parser.add_option("--showmessages", dest="showmessages", action='store_true',
-                  help="show incoming messages", default=False)
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-    import mavlink10 as mavlink
-else:
-    import mavlink
-import mavutil
-
-if opts.device is None:
-    print("You must specify a serial device")
-    sys.exit(1)
-
-def wait_heartbeat(m):
-    '''wait for a heartbeat so we know the target system IDs'''
-    print("Waiting for APM heartbeat")
-    m.wait_heartbeat()
-    print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system))
-
-def show_messages(m):
-    '''show incoming mavlink messages'''
-    while True:
-        msg = m.recv_match(blocking=True)
-        if not msg:
-            return
-        if msg.get_type() == "BAD_DATA":
-            if mavutil.all_printable(msg.data):
-                sys.stdout.write(msg.data)
-                sys.stdout.flush()
-        else:
-            print(msg)                    
-                
-# create a mavlink serial instance
-master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate)
-
-# wait for the heartbeat msg to find the system ID
-wait_heartbeat(master)
-
-print("Sending all stream request for rate %u" % opts.rate)
-for i in range(0, 3):
-    master.mav.request_data_stream_send(master.target_system, master.target_component,
-                                        mavlink.MAV_DATA_STREAM_ALL, opts.rate, 1)
-if opts.showmessages:
-    show_messages(master)
diff --git a/mavlink/share/pyshared/pymavlink/examples/bwtest.py b/mavlink/share/pyshared/pymavlink/examples/bwtest.py
deleted file mode 100644
index de56d4f8c..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/bwtest.py
+++ /dev/null
@@ -1,58 +0,0 @@
-#!/usr/bin/env python
-
-'''
-check bandwidth of link
-'''
-
-import sys, struct, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-import mavutil
-
-from optparse import OptionParser
-parser = OptionParser("bwtest.py [options]")
-
-parser.add_option("--baudrate", dest="baudrate", type='int',
-                  help="master port baud rate", default=115200)
-parser.add_option("--device", dest="device", default=None, help="serial device")
-(opts, args) = parser.parse_args()
-
-if opts.device is None:
-    print("You must specify a serial device")
-    sys.exit(1)
-
-# create a mavlink serial instance
-master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate)
-
-t1 = time.time()
-
-counts = {}
-
-bytes_sent = 0
-bytes_recv = 0
-
-while True:
-    master.mav.heartbeat_send(1, 1)
-    master.mav.sys_status_send(1, 2, 3, 4, 5, 6, 7)
-    master.mav.gps_raw_send(1, 2, 3, 4, 5, 6, 7, 8, 9)
-    master.mav.attitude_send(1, 2, 3, 4, 5, 6, 7)
-    master.mav.vfr_hud_send(1, 2, 3, 4, 5, 6)
-    while master.port.inWaiting() > 0:
-        m = master.recv_msg()
-        if m == None: break
-        if m.get_type() not in counts:
-            counts[m.get_type()] = 0
-        counts[m.get_type()] += 1
-    t2 = time.time()
-    if t2 - t1 > 1.0:
-        print("%u sent, %u received, %u errors bwin=%.1f kB/s bwout=%.1f kB/s" % (
-            master.mav.total_packets_sent,
-            master.mav.total_packets_received,
-            master.mav.total_receive_errors,
-            0.001*(master.mav.total_bytes_received-bytes_recv)/(t2-t1),
-            0.001*(master.mav.total_bytes_sent-bytes_sent)/(t2-t1)))
-        bytes_sent = master.mav.total_bytes_sent
-        bytes_recv = master.mav.total_bytes_received
-        t1 = t2
diff --git a/mavlink/share/pyshared/pymavlink/examples/flightmodes.py b/mavlink/share/pyshared/pymavlink/examples/flightmodes.py
deleted file mode 100644
index 03d7e2c47..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/flightmodes.py
+++ /dev/null
@@ -1,52 +0,0 @@
-#!/usr/bin/env python
-
-'''
-show changes in flight modes
-'''
-
-import sys, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("flightmodes.py [options]")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    print("Usage: flightmodes.py [options] <LOGFILE...>")
-    sys.exit(1)
-
-def flight_modes(logfile):
-    '''show flight modes for a log file'''
-    print("Processing log %s" % filename)
-    mlog = mavutil.mavlink_connection(filename)
-
-    mode = -1
-    nav_mode = -1
-
-    filesize = os.path.getsize(filename)
-
-    while True:
-        m = mlog.recv_match(type='SYS_STATUS',
-                            condition='SYS_STATUS.mode != %u or SYS_STATUS.nav_mode != %u' % (mode, nav_mode))
-        if m is None:
-            return
-        mode = m.mode
-        nav_mode = m.nav_mode
-        pct = (100.0 * mlog.f.tell()) / filesize
-        print('%s MAV.flightmode=%-12s mode=%u nav_mode=%u (MAV.timestamp=%u %u%%)' % (
-            time.asctime(time.localtime(m._timestamp)),
-            mlog.flightmode,
-            mode, nav_mode, m._timestamp, pct))
-
-for filename in args:
-    flight_modes(filename)
-
-
diff --git a/mavlink/share/pyshared/pymavlink/examples/flighttime.py b/mavlink/share/pyshared/pymavlink/examples/flighttime.py
deleted file mode 100644
index 81cd38c94..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/flighttime.py
+++ /dev/null
@@ -1,59 +0,0 @@
-#!/usr/bin/env python
-
-'''
-work out total flight time for a mavlink log
-'''
-
-import sys, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("flighttime.py [options]")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    print("Usage: flighttime.py [options] <LOGFILE...>")
-    sys.exit(1)
-
-def flight_time(logfile):
-    '''work out flight time for a log file'''
-    print("Processing log %s" % filename)
-    mlog = mavutil.mavlink_connection(filename)
-
-    in_air = False
-    start_time = 0.0
-    total_time = 0.0
-    t = None
-
-    while True:
-        m = mlog.recv_match(type='VFR_HUD')
-        if m is None:
-            if in_air:
-                total_time += time.mktime(t) - start_time
-            if total_time > 0:
-                print("Flight time : %u:%02u" % (int(total_time)/60, int(total_time)%60))
-            return total_time
-        t = time.localtime(m._timestamp)
-        if m.groundspeed > 3.0 and not in_air:
-            print("In air at %s" % time.asctime(t))
-            in_air = True
-            start_time = time.mktime(t)
-        elif m.groundspeed < 3.0 and in_air:
-            print("On ground at %s" % time.asctime(t))
-            in_air = False
-            total_time += time.mktime(t) - start_time
-    return total_time
-
-total = 0.0
-for filename in args:
-    total += flight_time(filename)
-
-print("Total time in air: %u:%02u" % (int(total)/60, int(total)%60))
diff --git a/mavlink/share/pyshared/pymavlink/examples/gpslock.py b/mavlink/share/pyshared/pymavlink/examples/gpslock.py
deleted file mode 100644
index f15b29072..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/gpslock.py
+++ /dev/null
@@ -1,68 +0,0 @@
-#!/usr/bin/env python
-
-'''
-show GPS lock events in a MAVLink log
-'''
-
-import sys, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("gpslock.py [options]")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    print("Usage: gpslock.py [options] <LOGFILE...>")
-    sys.exit(1)
-
-def lock_time(logfile):
-    '''work out gps lock times for a log file'''
-    print("Processing log %s" % filename)
-    mlog = mavutil.mavlink_connection(filename)
-
-    locked = False
-    start_time = 0.0
-    total_time = 0.0
-    t = None
-    m = mlog.recv_match(type='GPS_RAW')
-    unlock_time = time.mktime(time.localtime(m._timestamp))
-
-    while True:
-        m = mlog.recv_match(type='GPS_RAW')
-        if m is None:
-            if locked:
-                total_time += time.mktime(t) - start_time
-            if total_time > 0:
-                print("Lock time : %u:%02u" % (int(total_time)/60, int(total_time)%60))
-            return total_time
-        t = time.localtime(m._timestamp)
-        if m.fix_type == 2 and not locked:
-            print("Locked at %s after %u seconds" % (time.asctime(t),
-                                                     time.mktime(t) - unlock_time))
-            locked = True
-            start_time = time.mktime(t)
-        elif m.fix_type == 1 and locked:
-            print("Lost GPS lock at %s" % time.asctime(t))
-            locked = False
-            total_time += time.mktime(t) - start_time
-            unlock_time = time.mktime(t)
-        elif m.fix_type == 0 and locked:
-            print("Lost protocol lock at %s" % time.asctime(t))
-            locked = False
-            total_time += time.mktime(t) - start_time
-            unlock_time = time.mktime(t)
-    return total_time
-
-total = 0.0
-for filename in args:
-    total += lock_time(filename)
-
-print("Total time locked: %u:%02u" % (int(total)/60, int(total)%60))
diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit.py b/mavlink/share/pyshared/pymavlink/examples/magfit.py
deleted file mode 100644
index 7bfda796b..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/magfit.py
+++ /dev/null
@@ -1,138 +0,0 @@
-#!/usr/bin/env python
-
-'''
-fit best estimate of magnetometer offsets
-'''
-
-import sys, time, os, math
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("magfit.py [options]")
-parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
-parser.add_option("--condition",dest="condition", default=None, help="select packets by condition")
-parser.add_option("--noise", type='float', default=0, help="noise to add")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-from rotmat import Vector3
-
-if len(args) < 1:
-    print("Usage: magfit.py [options] <LOGFILE...>")
-    sys.exit(1)
-
-def noise():
-    '''a noise vector'''
-    from random import gauss
-    v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
-    v.normalize()
-    return v * opts.noise
-
-def select_data(data):
-    ret = []
-    counts = {}
-    for d in data:
-        mag = d
-        key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20)
-        if key in counts:
-            counts[key] += 1
-        else:
-            counts[key] = 1
-        if counts[key] < 3:
-            ret.append(d)
-    print(len(data), len(ret))
-    return ret
-
-def radius(mag, offsets):
-    '''return radius give data point and offsets'''
-    return (mag + offsets).length()
-
-def radius_cmp(a, b, offsets):
-    '''return radius give data point and offsets'''
-    diff = radius(a, offsets) - radius(b, offsets)
-    if diff > 0:
-        return 1
-    if diff < 0:
-        return -1
-    return 0
-
-def sphere_error(p, data):
-    from scipy import sqrt
-    x,y,z,r = p
-    ofs = Vector3(x,y,z)
-    ret = []
-    for d in data:
-        mag = d
-        err = r - radius(mag, ofs)
-        ret.append(err)
-    return ret
-
-def fit_data(data):
-    import numpy, scipy
-    from scipy import optimize
-
-    p0 = [0.0, 0.0, 0.0, 0.0]
-    p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data))
-    if not ier in [1, 2, 3, 4]:
-        raise RuntimeError("Unable to find solution")
-    return (Vector3(p1[0], p1[1], p1[2]), p1[3])
-
-def magfit(logfile):
-    '''find best magnetometer offset fit to a log file'''
-
-    print("Processing log %s" % filename)
-    mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
-
-    data = []
-
-    last_t = 0
-    offsets = Vector3(0,0,0)
-
-    # now gather all the data
-    while True:
-        m = mlog.recv_match(condition=opts.condition)
-        if m is None:
-            break
-        if m.get_type() == "SENSOR_OFFSETS":
-            # update current offsets
-            offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
-        if m.get_type() == "RAW_IMU":
-            mag = Vector3(m.xmag, m.ymag, m.zmag)
-            # add data point after subtracting the current offsets
-            data.append(mag - offsets + noise())
-
-    print("Extracted %u data points" % len(data))
-    print("Current offsets: %s" % offsets)
-
-    data = select_data(data)
-
-    # do an initial fit with all data
-    (offsets, field_strength) = fit_data(data)
-
-    for count in range(3):
-        # sort the data by the radius
-        data.sort(lambda a,b : radius_cmp(a,b,offsets))
-
-        print("Fit %u    : %s  field_strength=%6.1f to %6.1f" % (
-            count, offsets,
-            radius(data[0], offsets), radius(data[-1], offsets)))
-        
-        # discard outliers, keep the middle 3/4
-        data = data[len(data)/8:-len(data)/8]
-
-        # fit again
-        (offsets, field_strength) = fit_data(data)
-
-    print("Final    : %s  field_strength=%6.1f to %6.1f" % (
-        offsets,
-        radius(data[0], offsets), radius(data[-1], offsets)))
-
-total = 0.0
-for filename in args:
-    magfit(filename)
diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py b/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py
deleted file mode 100644
index 87d2dbb7f..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py
+++ /dev/null
@@ -1,145 +0,0 @@
-#!/usr/bin/env python
-
-'''
-fit best estimate of magnetometer offsets using the algorithm from
-Bill Premerlani
-'''
-
-import sys, time, os, math
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-# command line option handling
-from optparse import OptionParser
-parser = OptionParser("magfit_delta.py [options]")
-parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
-parser.add_option("--condition",dest="condition", default=None, help="select packets by condition")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-parser.add_option("--verbose", action='store_true', default=False, help="verbose offset output")
-parser.add_option("--gain", type='float', default=0.01, help="algorithm gain")
-parser.add_option("--noise", type='float', default=0, help="noise to add")
-parser.add_option("--max-change", type='float', default=10, help="max step change")
-parser.add_option("--min-diff", type='float', default=50, help="min mag vector delta")
-parser.add_option("--history", type='int', default=20, help="how many points to keep")
-parser.add_option("--repeat", type='int', default=1, help="number of repeats through the data")
-
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-from rotmat import Vector3, Matrix3
-
-if len(args) < 1:
-    print("Usage: magfit_delta.py [options] <LOGFILE...>")
-    sys.exit(1)
-
-def noise():
-    '''a noise vector'''
-    from random import gauss
-    v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
-    v.normalize()
-    return v * opts.noise
-
-def find_offsets(data, ofs):
-    '''find mag offsets by applying Bills "offsets revisited" algorithm
-       on the data
-
-       This is an implementation of the algorithm from:
-          http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
-       '''
-
-    # a limit on the maximum change in each step
-    max_change = opts.max_change
-
-    # the gain factor for the algorithm
-    gain = opts.gain
-
-    data2 = []
-    for d in data:
-        d = d.copy() + noise()
-        d.x = float(int(d.x + 0.5))
-        d.y = float(int(d.y + 0.5))
-        d.z = float(int(d.z + 0.5))
-        data2.append(d)
-    data = data2
-
-    history_idx = 0
-    mag_history = data[0:opts.history]
-    
-    for i in range(opts.history, len(data)):
-        B1 = mag_history[history_idx] + ofs
-        B2 = data[i] + ofs
-        
-        diff = B2 - B1
-        diff_length = diff.length()
-        if diff_length <= opts.min_diff:
-            # the mag vector hasn't changed enough - we don't get any
-            # information from this
-            history_idx = (history_idx+1) % opts.history
-            continue
-
-        mag_history[history_idx] = data[i]
-        history_idx = (history_idx+1) % opts.history
-
-        # equation 6 of Bills paper
-        delta = diff * (gain * (B2.length() - B1.length()) / diff_length)
-
-        # limit the change from any one reading. This is to prevent
-        # single crazy readings from throwing off the offsets for a long
-        # time
-        delta_length = delta.length()
-        if max_change != 0 and delta_length > max_change:
-            delta *= max_change / delta_length
-
-        # set the new offsets
-        ofs = ofs - delta
-
-        if opts.verbose:
-            print ofs
-    return ofs
-
-
-def magfit(logfile):
-    '''find best magnetometer offset fit to a log file'''
-
-    print("Processing log %s" % filename)
-
-    # open the log file
-    mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
-
-    data = []
-    mag = None
-    offsets = Vector3(0,0,0)
-    
-    # now gather all the data
-    while True:
-        # get the next MAVLink message in the log
-        m = mlog.recv_match(condition=opts.condition)
-        if m is None:
-            break
-        if m.get_type() == "SENSOR_OFFSETS":
-            # update offsets that were used during this flight
-            offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
-        if m.get_type() == "RAW_IMU" and offsets != None:
-            # extract one mag vector, removing the offsets that were
-            # used during that flight to get the raw sensor values
-            mag = Vector3(m.xmag, m.ymag, m.zmag) - offsets
-            data.append(mag)
-
-    print("Extracted %u data points" % len(data))
-    print("Current offsets: %s" % offsets)
-
-    # run the fitting algorithm
-    ofs = offsets
-    ofs = Vector3(0,0,0)
-    for r in range(opts.repeat):
-        ofs = find_offsets(data, ofs)
-        print('Loop %u offsets %s' % (r, ofs))
-        sys.stdout.flush()
-    print("New offsets: %s" % ofs)
-
-total = 0.0
-for filename in args:
-    magfit(filename)
diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py b/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py
deleted file mode 100644
index 30ba45806..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py
+++ /dev/null
@@ -1,159 +0,0 @@
-#!/usr/bin/env python
-
-'''
-fit best estimate of magnetometer offsets
-'''
-
-import sys, time, os, math
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("magfit.py [options]")
-parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-parser.add_option("--minspeed", type='float', default=5.0, help="minimum ground speed to use")
-
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    print("Usage: magfit.py [options] <LOGFILE...>")
-    sys.exit(1)
-
-class vec3(object):
-    def __init__(self, x, y, z):
-        self.x = x
-        self.y = y
-        self.z = z
-    def __str__(self):
-        return "%.1f %.1f %.1f" % (self.x, self.y, self.z)
-
-def heading_error1(parm, data):
-    from math import sin, cos, atan2, degrees
-    from numpy import dot
-    xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm
-
-    ret = []
-    for d in data:
-        x = d[0] + xofs
-        y = d[1] + yofs
-        z = d[2] + zofs
-        r = d[3]
-        p = d[4]
-        h = d[5]
-
-        headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p)
-        headY = y*cos(r) - z*sin(r)
-        heading = degrees(atan2(-headY,headX)) + declination
-        if heading < 0:
-            heading += 360
-        herror = h - heading
-        if herror > 180:
-            herror -= 360
-        if herror < -180:
-            herror += 360
-        ret.append(herror)
-    return ret
-
-def heading_error(parm, data):
-    from math import sin, cos, atan2, degrees
-    from numpy import dot
-    xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm
-
-    a = [[1.0,a2,a3],[a4,a5,a6],[a7,a8,a9]]
-
-    ret = []
-    for d in data:
-        x = d[0] + xofs
-        y = d[1] + yofs
-        z = d[2] + zofs
-        r = d[3]
-        p = d[4]
-        h = d[5]
-        mv = [x, y, z]
-        mv2 = dot(a, mv)
-        x = mv2[0]
-        y = mv2[1]
-        z = mv2[2]
-
-        headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p)
-        headY = y*cos(r) - z*sin(r)
-        heading = degrees(atan2(-headY,headX)) + declination
-        if heading < 0:
-            heading += 360
-        herror = h - heading
-        if herror > 180:
-            herror -= 360
-        if herror < -180:
-            herror += 360
-        ret.append(herror)
-    return ret
-
-def fit_data(data):
-    import numpy, scipy
-    from scipy import optimize
-
-    p0 = [0.0, 0.0, 0.0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0]
-    p1, ier = optimize.leastsq(heading_error1, p0[:], args=(data))
-
-#    p0 = p1[:]
-#    p1, ier = optimize.leastsq(heading_error, p0[:], args=(data))
-
-    print(p1)
-    if not ier in [1, 2, 3, 4]:
-        raise RuntimeError("Unable to find solution")
-    return p1
-
-def magfit(logfile):
-    '''find best magnetometer offset fit to a log file'''
-    print("Processing log %s" % filename)
-    mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
-
-    flying = False
-    gps_heading = 0.0
-
-    data = []
-
-    # get the current mag offsets
-    m = mlog.recv_match(type='SENSOR_OFFSETS')
-    offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
-
-    attitude = mlog.recv_match(type='ATTITUDE')
-
-    # now gather all the data
-    while True:
-        m = mlog.recv_match()
-        if m is None:
-            break
-        if m.get_type() == "GPS_RAW":
-            # flying if groundspeed more than 5 m/s
-            flying = (m.v > opts.minspeed and m.fix_type == 2)
-            gps_heading = m.hdg
-        if m.get_type() == "ATTITUDE":
-            attitude = m
-        if m.get_type() == "SENSOR_OFFSETS":
-            # update current offsets
-            offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
-        if not flying:
-            continue
-        if m.get_type() == "RAW_IMU":
-            data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading))
-    print("Extracted %u data points" % len(data))
-    print("Current offsets: %s" % offsets)
-    ofs2 = fit_data(data)
-    print("Declination estimate: %.1f" % ofs2[-1])
-    new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2])
-    a = [[ofs2[3], ofs2[4], ofs2[5]],
-         [ofs2[6], ofs2[7], ofs2[8]],
-         [ofs2[9], ofs2[10], ofs2[11]]]
-    print(a)
-    print("New offsets    : %s" % new_offsets)
-
-total = 0.0
-for filename in args:
-    magfit(filename)
diff --git a/mavlink/share/pyshared/pymavlink/examples/magtest.py b/mavlink/share/pyshared/pymavlink/examples/magtest.py
deleted file mode 100644
index 8b910f8fd..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/magtest.py
+++ /dev/null
@@ -1,120 +0,0 @@
-#!/usr/bin/env python
-
-'''
-rotate APMs on bench to test magnetometers
-
-'''
-
-import sys, os, time
-from math import radians
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-import mavlink, mavutil
-
-from optparse import OptionParser
-parser = OptionParser("rotate.py [options]")
-
-parser.add_option("--device1", dest="device1", default=None, help="mavlink device1")
-parser.add_option("--device2", dest="device2", default=None, help="mavlink device2")
-parser.add_option("--baudrate", dest="baudrate", type='int',
-                  help="master port baud rate", default=115200)
-(opts, args) = parser.parse_args()
-
-if opts.device1 is None or opts.device2 is None:
-    print("You must specify a mavlink device")
-    sys.exit(1)
-
-def set_attitude(rc3, rc4):
-    global mav1, mav2
-    values = [ 65535 ] * 8
-    values[2] = rc3
-    values[3] = rc4
-    mav1.mav.rc_channels_override_send(mav1.target_system, mav1.target_component, *values)
-    mav2.mav.rc_channels_override_send(mav2.target_system, mav2.target_component, *values)
-
-
-# create a mavlink instance
-mav1 = mavutil.mavlink_connection(opts.device1, baud=opts.baudrate)
-
-# create a mavlink instance
-mav2 = mavutil.mavlink_connection(opts.device2, baud=opts.baudrate)
-
-print("Waiting for HEARTBEAT")
-mav1.wait_heartbeat()
-mav2.wait_heartbeat()
-print("Heartbeat from APM (system %u component %u)" % (mav1.target_system, mav1.target_system))
-print("Heartbeat from APM (system %u component %u)" % (mav2.target_system, mav2.target_system))
-
-print("Waiting for MANUAL mode")
-mav1.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True)
-mav2.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True)
-
-print("Setting declination")
-mav1.mav.param_set_send(mav1.target_system, mav1.target_component,
-                       'COMPASS_DEC', radians(12.33))
-mav2.mav.param_set_send(mav2.target_system, mav2.target_component,
-                       'COMPASS_DEC', radians(12.33))
-
-
-set_attitude(1060, 1160)
-
-event = mavutil.periodic_event(30)
-pevent = mavutil.periodic_event(0.3)
-rc3_min = 1060
-rc3_max = 1850
-rc4_min = 1080
-rc4_max = 1500
-rc3 = rc3_min
-rc4 = 1160
-delta3 = 2
-delta4 = 1
-use_pitch = 1
-
-MAV_ACTION_CALIBRATE_GYRO = 17
-mav1.mav.action_send(mav1.target_system, mav1.target_component, MAV_ACTION_CALIBRATE_GYRO)
-mav2.mav.action_send(mav2.target_system, mav2.target_component, MAV_ACTION_CALIBRATE_GYRO)
-
-print("Waiting for gyro calibration")
-mav1.recv_match(type='ACTION_ACK')
-mav2.recv_match(type='ACTION_ACK')
-
-print("Resetting mag offsets")
-mav1.mav.set_mag_offsets_send(mav1.target_system, mav1.target_component, 0, 0, 0)
-mav2.mav.set_mag_offsets_send(mav2.target_system, mav2.target_component, 0, 0, 0)
-
-def TrueHeading(SERVO_OUTPUT_RAW):
-    p = float(SERVO_OUTPUT_RAW.servo3_raw - rc3_min) / (rc3_max - rc3_min)
-    return 172 + p*(326 - 172)
-
-while True:
-    mav1.recv_msg()
-    mav2.recv_msg()
-    if event.trigger():
-        if not use_pitch:
-            rc4 = 1160
-        set_attitude(rc3, rc4)
-        rc3 += delta3
-        if rc3 > rc3_max or rc3 < rc3_min:
-            delta3 = -delta3
-            use_pitch ^= 1
-        rc4 += delta4
-        if rc4 > rc4_max or rc4 < rc4_min:
-            delta4 = -delta4
-    if pevent.trigger():
-        print "hdg1: %3u hdg2: %3u  ofs1: %4u, %4u, %4u  ofs2: %4u, %4u, %4u" % (
-            mav1.messages['VFR_HUD'].heading,
-            mav2.messages['VFR_HUD'].heading,
-            mav1.messages['SENSOR_OFFSETS'].mag_ofs_x,
-            mav1.messages['SENSOR_OFFSETS'].mag_ofs_y,
-            mav1.messages['SENSOR_OFFSETS'].mag_ofs_z,
-            mav2.messages['SENSOR_OFFSETS'].mag_ofs_x,
-            mav2.messages['SENSOR_OFFSETS'].mag_ofs_y,
-            mav2.messages['SENSOR_OFFSETS'].mag_ofs_z,
-            )
-    time.sleep(0.01)
-
-# 314M 326G
-# 160M 172G
-
diff --git a/mavlink/share/pyshared/pymavlink/examples/mavgraph.py b/mavlink/share/pyshared/pymavlink/examples/mavgraph.py
deleted file mode 100644
index e19856487..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/mavgraph.py
+++ /dev/null
@@ -1,196 +0,0 @@
-#!/usr/bin/env python
-'''
-graph a MAVLink log file
-Andrew Tridgell August 2011
-'''
-
-import sys, struct, time, os, datetime
-import math, re
-import pylab, pytz, matplotlib
-from math import *
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from mavextra import *
-
-locator = None
-formatter = None
-
-def plotit(x, y, fields, colors=[]):
-    '''plot a set of graphs using date for x axis'''
-    global locator, formatter
-    pylab.ion()
-    fig = pylab.figure(num=1, figsize=(12,6))
-    ax1 = fig.gca()
-    ax2 = None
-    xrange = 0.0
-    for i in range(0, len(fields)):
-        if len(x[i]) == 0: continue
-        if x[i][-1] - x[i][0] > xrange:
-            xrange = x[i][-1] - x[i][0]
-    xrange *= 24 * 60 * 60
-    if formatter is None:
-        if xrange < 1000:
-            formatter = matplotlib.dates.DateFormatter('%H:%M:%S')
-        else:
-            formatter = matplotlib.dates.DateFormatter('%H:%M')
-        interval = 1
-        intervals = [ 1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600,
-                      900, 1800, 3600, 7200, 5*3600, 10*3600, 24*3600 ]
-        for interval in intervals:
-            if xrange / interval < 15:
-                break
-        locator = matplotlib.dates.SecondLocator(interval=interval)
-    ax1.xaxis.set_major_locator(locator)
-    ax1.xaxis.set_major_formatter(formatter)
-    empty = True
-    ax1_labels = []
-    ax2_labels = []
-    for i in range(0, len(fields)):
-        if len(x[i]) == 0:
-            print("Failed to find any values for field %s" % fields[i])
-            continue
-        if i < len(colors):
-            color = colors[i]
-        else:
-            color = 'red'
-        (tz, tzdst) = time.tzname
-        if axes[i] == 2:
-            if ax2 == None:
-                ax2 = ax1.twinx()
-            ax = ax2
-            ax2.xaxis.set_major_locator(locator)
-            ax2.xaxis.set_major_formatter(formatter)
-            label = fields[i]
-            if label.endswith(":2"):
-                label = label[:-2]
-            ax2_labels.append(label)
-        else:
-            ax1_labels.append(fields[i])
-            ax = ax1
-        ax.plot_date(x[i], y[i], color=color, label=fields[i],
-                     linestyle='-', marker='None', tz=None)
-        pylab.draw()
-        empty = False
-    if ax1_labels != []:
-        ax1.legend(ax1_labels,loc=opts.legend)
-    if ax2_labels != []:
-        ax2.legend(ax2_labels,loc=opts.legend2)
-    if empty:
-        print("No data to graph")
-        return
-
-
-from optparse import OptionParser
-parser = OptionParser("mavgraph.py [options] <filename> <fields>")
-
-parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
-parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format")
-parser.add_option("--condition",dest="condition", default=None, help="select packets by a condition")
-parser.add_option("--labels",dest="labels", default=None, help="comma separated field labels")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-parser.add_option("--legend",  default='upper left', help="default legend position")
-parser.add_option("--legend2",  default='upper right', help="default legend2 position")
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 2:
-    print("Usage: mavlogdump.py [options] <LOGFILES...> <fields...>")
-    sys.exit(1)
-
-filenames = []
-fields = []
-for f in args:
-    if os.path.exists(f):
-        filenames.append(f)
-    else:
-        fields.append(f)
-msg_types = set()
-multiplier = []
-field_types = []
-
-colors = [ 'red', 'green', 'blue', 'orange', 'olive', 'black', 'grey' ]
-
-# work out msg types we are interested in
-x = []
-y = []
-axes = []
-first_only = []
-re_caps = re.compile('[A-Z_]+')
-for f in fields:
-    caps = set(re.findall(re_caps, f))
-    msg_types = msg_types.union(caps)
-    field_types.append(caps)
-    y.append([])
-    x.append([])
-    axes.append(1)
-    first_only.append(False)
-
-def add_data(t, msg, vars):
-    '''add some data'''
-    mtype = msg.get_type()
-    if mtype not in msg_types:
-        return
-    for i in range(0, len(fields)):
-        if mtype not in field_types[i]:
-            continue
-        f = fields[i]
-        if f.endswith(":2"):
-            axes[i] = 2
-            f = f[:-2]
-        if f.endswith(":1"):
-            first_only[i] = True
-            f = f[:-2]
-        v = mavutil.evaluate_expression(f, vars)
-        if v is None:
-            continue
-        y[i].append(v)
-        x[i].append(t)
-
-def process_file(filename):
-    '''process one file'''
-    print("Processing %s" % filename)
-    mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
-    vars = {}
-    
-    while True:
-        msg = mlog.recv_match(opts.condition)
-        if msg is None: break
-        tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60)
-        tdays += 719163 # pylab wants it since 0001-01-01
-        add_data(tdays, msg, mlog.messages)
-
-if len(filenames) == 0:
-    print("No files to process")
-    sys.exit(1)
-
-if opts.labels is not None:
-    labels = opts.labels.split(',')
-    if len(labels) != len(fields)*len(filenames):
-        print("Number of labels (%u) must match number of fields (%u)" % (
-            len(labels), len(fields)*len(filenames)))
-        sys.exit(1)
-else:
-    labels = None
-
-for fi in range(0, len(filenames)):
-    f = filenames[fi]
-    process_file(f)
-    for i in range(0, len(x)):
-        if first_only[i] and fi != 0:
-            x[i] = []
-            y[i] = []
-    if labels:
-        lab = labels[fi*len(fields):(fi+1)*len(fields)]
-    else:
-        lab = fields[:]
-    plotit(x, y, lab, colors=colors[fi*len(fields):])
-    for i in range(0, len(x)):
-        x[i] = []
-        y[i] = []
-pylab.show()
-raw_input('press enter to exit....')
diff --git a/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py b/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py
deleted file mode 100644
index f4cdc56bf..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py
+++ /dev/null
@@ -1,67 +0,0 @@
-#!/usr/bin/env python
-
-'''
-example program that dumps a Mavlink log file. The log file is
-assumed to be in the format that qgroundcontrol uses, which consists
-of a series of MAVLink packets, each with a 64 bit timestamp
-header. The timestamp is in microseconds since 1970 (unix epoch)
-'''
-
-import sys, time, os, struct
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("mavlogdump.py [options]")
-
-parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
-parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format")
-parser.add_option("--robust",dest="robust", action='store_true', help="Enable robust parsing (skip over bad data)")
-parser.add_option("-f", "--follow",dest="follow", action='store_true', help="keep waiting for more data at end of file")
-parser.add_option("--condition",dest="condition", default=None, help="select packets by condition")
-parser.add_option("-q", "--quiet", dest="quiet", action='store_true', help="don't display packets")
-parser.add_option("-o", "--output", default=None, help="output matching packets to give file")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-parser.add_option("--types",  default=None, help="types of messages (comma separated)")
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    print("Usage: mavlogdump.py [options] <LOGFILE>")
-    sys.exit(1)
-
-filename = args[0]
-mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner,
-                                  notimestamps=opts.notimestamps,
-                                  robust_parsing=opts.robust)
-
-output = None
-if opts.output:
-    output = mavutil.mavlogfile(opts.output, write=True)
-
-types = opts.types
-if types is not None:
-    types = types.split(',')
-    
-while True:
-    m = mlog.recv_match(condition=opts.condition, blocking=opts.follow)
-    if m is None:
-        break
-    if types is not None and m.get_type() not in types:
-        continue
-    if output:
-        timestamp = getattr(m, '_timestamp', None)
-        if timestamp:
-            output.write(struct.pack('>Q', timestamp*1.0e6))
-        output.write(m.get_msgbuf().tostring())
-    if opts.quiet:
-        continue
-    print("%s.%02u: %s" % (
-        time.strftime("%Y-%m-%d %H:%M:%S",
-                      time.localtime(m._timestamp)),
-        int(m._timestamp*100.0)%100, m))
-        
diff --git a/mavlink/share/pyshared/pymavlink/examples/mavparms.py b/mavlink/share/pyshared/pymavlink/examples/mavparms.py
deleted file mode 100644
index 702fbd9e1..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/mavparms.py
+++ /dev/null
@@ -1,48 +0,0 @@
-#!/usr/bin/env python
-
-'''
-extract mavlink parameter values
-'''
-
-import sys, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("mavparms.py [options]")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    print("Usage: mavparms.py [options] <LOGFILE...>")
-    sys.exit(1)
-
-parms = {}
-
-def mavparms(logfile):
-    '''extract mavlink parameters'''
-    mlog = mavutil.mavlink_connection(filename)
-
-    while True:
-        m = mlog.recv_match(type='PARAM_VALUE')
-        if m is None:
-            return
-        pname = str(m.param_id).strip()
-        if len(pname) > 0:
-            parms[pname] = m.param_value
-
-total = 0.0
-for filename in args:
-    mavparms(filename)
-
-keys = parms.keys()
-keys.sort()
-for p in keys:
-    print("%-15s %.6f" % (p, parms[p]))
-    
diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtest.py b/mavlink/share/pyshared/pymavlink/examples/mavtest.py
deleted file mode 100644
index 3c385e48a..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/mavtest.py
+++ /dev/null
@@ -1,41 +0,0 @@
-#!/usr/bin/env python
-
-import sys, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-import mavlink
-
-class fifo(object):
-    def __init__(self):
-        self.buf = []
-    def write(self, data):
-        self.buf += data
-        return len(data)
-    def read(self):
-        return self.buf.pop(0)
-
-f = fifo()
-
-# create a mavlink instance, which will do IO on file object 'f'
-mav = mavlink.MAVLink(f)
-
-# set the WP_RADIUS parameter on the MAV at the end of the link
-mav.param_set_send(7, 1, "WP_RADIUS", 101)
-
-# alternatively, produce a MAVLink_param_set object 
-# this can be sent via your own transport if you like
-m = mav.param_set_encode(7, 1, "WP_RADIUS", 101)
-
-# get the encoded message as a buffer
-b = m.get_msgbuf()
-
-# decode an incoming message
-m2 = mav.decode(b)
-
-# show what fields it has
-print("Got a message with id %u and fields %s" % (m2.get_msgId(), m2.get_fieldnames()))
-
-# print out the fields
-print(m2)
diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtester.py b/mavlink/share/pyshared/pymavlink/examples/mavtester.py
deleted file mode 100644
index 8b5284f3f..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/mavtester.py
+++ /dev/null
@@ -1,43 +0,0 @@
-#!/usr/bin/env python
-
-'''
-test mavlink messages
-'''
-
-import sys, struct, time, os
-from curses import ascii
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-import mavlink, mavtest, mavutil
-
-from optparse import OptionParser
-parser = OptionParser("mavtester.py [options]")
-
-parser.add_option("--baudrate", dest="baudrate", type='int',
-                  help="master port baud rate", default=115200)
-parser.add_option("--device", dest="device", default=None, help="serial device")
-parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int',
-                  default=255, help='MAVLink source system for this GCS')
-(opts, args) = parser.parse_args()
-
-if opts.device is None:
-    print("You must specify a serial device")
-    sys.exit(1)
-
-def wait_heartbeat(m):
-    '''wait for a heartbeat so we know the target system IDs'''
-    print("Waiting for APM heartbeat")
-    msg = m.recv_match(type='HEARTBEAT', blocking=True)
-    print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system))
-
-# create a mavlink serial instance
-master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate, source_system=opts.SOURCE_SYSTEM)
-
-# wait for the heartbeat msg to find the system ID
-wait_heartbeat(master)
-
-print("Sending all message types")
-mavtest.generate_outputs(master.mav)
-
diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py b/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py
deleted file mode 100644
index 92d3cb51c..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py
+++ /dev/null
@@ -1,83 +0,0 @@
-#!/usr/bin/env python
-
-'''
-example program to extract GPS data from a mavlink log, and create a GPX
-file, for loading into google earth
-'''
-
-import sys, struct, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("mavtogpx.py [options]")
-parser.add_option("--condition",dest="condition", default=None, help="select packets by a condition")
-parser.add_option("--nofixcheck", default=False, action='store_true', help="don't check for GPS fix")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    print("Usage: mavtogpx.py <LOGFILE>")
-    sys.exit(1)
-
-def mav_to_gpx(infilename, outfilename):
-    '''convert a mavlink log file to a GPX file'''
-
-    mlog = mavutil.mavlink_connection(infilename)
-    outf = open(outfilename, mode='w')
-
-    def process_packet(m):
-        t = time.localtime(m._timestamp)
-        outf.write('''<trkpt lat="%s" lon="%s">
-  <ele>%s</ele>
-  <time>%s</time>
-  <course>%s</course>
-  <speed>%s</speed>
-  <fix>3d</fix>
-</trkpt>
-''' % (m.lat, m.lon, m.alt,
-       time.strftime("%Y-%m-%dT%H:%M:%SZ", t),
-       m.hdg, m.v))
-
-    def add_header():
-        outf.write('''<?xml version="1.0" encoding="UTF-8"?>
-<gpx
-  version="1.0"
-  creator="pymavlink"
-  xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
-  xmlns="http://www.topografix.com/GPX/1/0"
-  xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
-<trk>
-<trkseg>
-''')
-
-    def add_footer():
-        outf.write('''</trkseg>
-</trk>
-</gpx>
-''')
-
-    add_header()       
-
-    count=0
-    while True:
-        m = mlog.recv_match(type='GPS_RAW', condition=opts.condition)
-        if m is None: break
-        if m.fix_type != 2 and not opts.nofixcheck:
-            continue
-        if m.lat == 0.0 or m.lon == 0.0:
-            continue
-        process_packet(m)
-        count += 1
-    add_footer()
-    print("Created %s with %u points" % (outfilename, count))
-    
-
-for infilename in args:
-    outfilename = infilename + '.gpx'
-    mav_to_gpx(infilename, outfilename)
diff --git a/mavlink/share/pyshared/pymavlink/examples/rotmat.py b/mavlink/share/pyshared/pymavlink/examples/rotmat.py
deleted file mode 100644
index 6d5405949..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/rotmat.py
+++ /dev/null
@@ -1,269 +0,0 @@
-#!/usr/bin/env python
-#
-# vector3 and rotation matrix classes
-# This follows the conventions in the ArduPilot code,
-# and is essentially a python version of the AP_Math library
-#
-# Andrew Tridgell, March 2012
-#
-# This library is free software; you can redistribute it and/or modify it
-# under the terms of the GNU Lesser General Public License as published by the
-# Free Software Foundation; either version 2.1 of the License, or (at your
-# option) any later version.
-#
-# This library is distributed in the hope that it will be useful, but WITHOUT
-# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-# FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
-# for more details.
-#
-# You should have received a copy of the GNU Lesser General Public License
-# along with this library; if not, write to the Free Software Foundation,
-# Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
-
-'''rotation matrix class
-'''
-
-from math import sin, cos, sqrt, asin, atan2, pi, radians, acos
-
-class Vector3:
-    '''a vector'''
-    def __init__(self, x=None, y=None, z=None):
-        if x != None and y != None and z != None:
-            self.x = float(x)
-            self.y = float(y)
-            self.z = float(z)
-        elif x != None and len(x) == 3:
-            self.x = float(x[0])
-            self.y = float(x[1])
-            self.z = float(x[2])
-        elif x != None:
-            raise ValueError('bad initialiser')
-        else:
-            self.x = float(0)
-            self.y = float(0)
-            self.z = float(0)
-
-    def __repr__(self):
-        return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
-                                              self.y,
-                                              self.z)
-
-    def __add__(self, v):
-        return Vector3(self.x + v.x,
-                       self.y + v.y,
-                       self.z + v.z)
-
-    __radd__ = __add__
-
-    def __sub__(self, v):
-        return Vector3(self.x - v.x,
-                       self.y - v.y,
-                       self.z - v.z)
-
-    def __neg__(self):
-        return Vector3(-self.x, -self.y, -self.z)
-
-    def __rsub__(self, v):
-        return Vector3(v.x - self.x,
-                       v.y - self.y,
-                       v.z - self.z)
-
-    def __mul__(self, v):
-        if isinstance(v, Vector3):
-            '''dot product'''
-            return self.x*v.x + self.y*v.y + self.z*v.z
-        return Vector3(self.x * v,
-                       self.y * v,
-                       self.z * v)
-
-    __rmul__ = __mul__
-
-    def __div__(self, v):
-        return Vector3(self.x / v,
-                       self.y / v,
-                       self.z / v)
-
-    def __mod__(self, v):
-        '''cross product'''
-        return Vector3(self.y*v.z - self.z*v.y,
-                       self.z*v.x - self.x*v.z,
-                       self.x*v.y - self.y*v.x)
-
-    def __copy__(self):
-        return Vector3(self.x, self.y, self.z)
-
-    copy = __copy__
-
-    def length(self):
-        return sqrt(self.x**2 + self.y**2 + self.z**2)
-
-    def zero(self):
-        self.x = self.y = self.z = 0
-
-    def angle(self, v):
-        '''return the angle between this vector and another vector'''
-        return acos(self * v) / (self.length() * v.length())
-
-    def normalized(self):
-        return self / self.length()
-    
-    def normalize(self):
-        v = self.normalized()
-        self.x = v.x
-        self.y = v.y
-        self.z = v.z
-        
-class Matrix3:
-    '''a 3x3 matrix, intended as a rotation matrix'''
-    def __init__(self, a=None, b=None, c=None):
-        if a is not None and b is not None and c is not None:
-            self.a = a.copy()
-            self.b = b.copy()
-            self.c = c.copy()
-        else:
-            self.identity()
-
-    def __repr__(self):
-        return 'Matrix3((%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f))' % (
-            self.a.x, self.a.y, self.a.z,
-            self.b.x, self.b.y, self.b.z,
-            self.c.x, self.c.y, self.c.z)
-
-    def identity(self):
-        self.a = Vector3(1,0,0)
-        self.b = Vector3(0,1,0)
-        self.c = Vector3(0,0,1)
-
-    def transposed(self):
-        return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
-                       Vector3(self.a.y, self.b.y, self.c.y),
-                       Vector3(self.a.z, self.b.z, self.c.z))
-
-        
-    def from_euler(self, roll, pitch, yaw):
-        '''fill the matrix from Euler angles in radians'''
-        cp = cos(pitch)
-	sp = sin(pitch)
-	sr = sin(roll)
-	cr = cos(roll)
-	sy = sin(yaw)
-	cy = cos(yaw)
-
-	self.a.x = cp * cy
-	self.a.y = (sr * sp * cy) - (cr * sy)
-	self.a.z = (cr * sp * cy) + (sr * sy)
-	self.b.x = cp * sy
-	self.b.y = (sr * sp * sy) + (cr * cy)
-	self.b.z = (cr * sp * sy) - (sr * cy)
-	self.c.x = -sp
-	self.c.y = sr * cp
-	self.c.z = cr * cp
-
-
-    def to_euler(self):
-        '''find Euler angles for the matrix'''
-        if self.c.x >= 1.0:
-            pitch = pi
-        elif self.c.x <= -1.0:
-            pitch = -pi
-        else:
-            pitch = -asin(self.c.x)
-        roll = atan2(self.c.y, self.c.z)
-        yaw  = atan2(self.b.x, self.a.x)
-        return (roll, pitch, yaw)
-
-    def __add__(self, m):
-        return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c)
-
-    __radd__ = __add__
-
-    def __sub__(self, m):
-        return Matrix3(self.a - m.a, self.b - m.b, self.c - m.c)
-
-    def __rsub__(self, m):
-        return Matrix3(m.a - self.a, m.b - self.b, m.c - self.c)
-    
-    def __mul__(self, other):
-        if isinstance(other, Vector3):
-            v = other
-            return Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z,
-                           self.b.x * v.x + self.b.y * v.y + self.b.z * v.z,
-                           self.c.x * v.x + self.c.y * v.y + self.c.z * v.z)
-        elif isinstance(other, Matrix3):
-            m = other
-            return Matrix3(Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x,
-                                   self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y,
-                                   self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z),
-                           Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x,
-                                   self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y,
-                                   self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z),
-                           Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x,
-                                   self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y,
-                                   self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z))
-        v = other
-        return Matrix3(self.a * v, self.b * v, self.c * v)
-
-    def __div__(self, v):
-        return Matrix3(self.a / v, self.b / v, self.c / v)
-
-    def __neg__(self):
-        return Matrix3(-self.a, -self.b, -self.c)
-
-    def __copy__(self):
-        return Matrix3(self.a, self.b, self.c)
-
-    copy = __copy__
-
-    def rotate(self, g):
-        '''rotate the matrix by a given amount on 3 axes'''
-	temp_matrix = Matrix3()
-        a = self.a
-        b = self.b
-        c = self.c
-	temp_matrix.a.x = a.y * g.z - a.z * g.y
-	temp_matrix.a.y = a.z * g.x - a.x * g.z
-	temp_matrix.a.z = a.x * g.y - a.y * g.x
-	temp_matrix.b.x = b.y * g.z - b.z * g.y
-	temp_matrix.b.y = b.z * g.x - b.x * g.z
-	temp_matrix.b.z = b.x * g.y - b.y * g.x
-	temp_matrix.c.x = c.y * g.z - c.z * g.y
-	temp_matrix.c.y = c.z * g.x - c.x * g.z
-	temp_matrix.c.z = c.x * g.y - c.y * g.x
-        self.a += temp_matrix.a
-        self.b += temp_matrix.b
-        self.c += temp_matrix.c
-
-    def normalize(self):
-        '''re-normalise a rotation matrix'''
-	error = self.a * self.b
-	t0 = self.a - (self.b * (0.5 * error))
-	t1 = self.b - (self.a * (0.5 * error))
-	t2 = t0 % t1
-        self.a = t0 * (1.0 / t0.length())
-        self.b = t1 * (1.0 / t1.length())
-        self.c = t2 * (1.0 / t2.length())
-
-    def trace(self):
-        '''the trace of the matrix'''
-        return self.a.x + self.b.y + self.c.z
-
-def test_euler():
-    '''check that from_euler() and to_euler() are consistent'''
-    m = Matrix3()
-    from math import radians, degrees
-    for r in range(-179, 179, 3):
-        for p in range(-89, 89, 3):
-            for y in range(-179, 179, 3):
-                m.from_euler(radians(r), radians(p), radians(y))
-                (r2, p2, y2) = m.to_euler()
-                v1 = Vector3(r,p,y)
-                v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
-                diff = v1 - v2
-                if diff.length() > 1.0e-12:
-                    print('EULER ERROR:', v1, v2, diff.length())
-                    
-if __name__ == "__main__":
-    import doctest
-    doctest.testmod()
-    test_euler()
-    
diff --git a/mavlink/share/pyshared/pymavlink/examples/sigloss.py b/mavlink/share/pyshared/pymavlink/examples/sigloss.py
deleted file mode 100644
index feb189d97..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/sigloss.py
+++ /dev/null
@@ -1,57 +0,0 @@
-#!/usr/bin/env python
-
-'''
-show times when signal is lost
-'''
-
-import sys, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("sigloss.py [options]")
-parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
-parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format")
-parser.add_option("--robust",dest="robust", action='store_true', help="Enable robust parsing (skip over bad data)")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-parser.add_option("--deltat", type='float', default=1.0, help="loss threshold in seconds")
-
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    print("Usage: sigloss.py [options] <LOGFILE...>")
-    sys.exit(1)
-
-def sigloss(logfile):
-    '''work out signal loss times for a log file'''
-    print("Processing log %s" % filename)
-    mlog = mavutil.mavlink_connection(filename,
-                                      planner_format=opts.planner,
-                                      notimestamps=opts.notimestamps,
-                                      robust_parsing=opts.robust)
-
-    last_t = 0
-
-    while True:
-        m = mlog.recv_match()
-        if m is None:
-            return
-        if opts.notimestamps:
-            if not 'usec' in m._fieldnames:
-                continue
-            t = m.usec / 1.0e6
-        else:
-            t = m._timestamp
-        if last_t != 0:
-            if t - last_t > opts.deltat:
-                print("Sig lost for %.1fs at %s" % (t-last_t, time.asctime(time.localtime(t))))
-        last_t = t
-
-total = 0.0
-for filename in args:
-    sigloss(filename)
diff --git a/mavlink/share/pyshared/pymavlink/examples/wptogpx.py b/mavlink/share/pyshared/pymavlink/examples/wptogpx.py
deleted file mode 100644
index 306f20af2..000000000
--- a/mavlink/share/pyshared/pymavlink/examples/wptogpx.py
+++ /dev/null
@@ -1,69 +0,0 @@
-#!/usr/bin/env python
-
-'''
-example program to extract GPS data from a waypoint file, and create a GPX
-file, for loading into google earth
-'''
-
-import sys, struct, time, os
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-from optparse import OptionParser
-parser = OptionParser("wptogpx.py [options]")
-(opts, args) = parser.parse_args()
-
-import mavutil, mavwp
-
-if len(args) < 1:
-    print("Usage: wptogpx.py <WPFILE>")
-    sys.exit(1)
-
-def wp_to_gpx(infilename, outfilename):
-    '''convert a wp file to a GPX file'''
-
-    wp = mavwp.MAVWPLoader()
-    wp.load(infilename)
-    outf = open(outfilename, mode='w')
-
-    def process_wp(w, i):
-        t = time.localtime(i)
-        outf.write('''<wpt lat="%s" lon="%s">
-  <ele>%s</ele>
-  <cmt>WP %u</cmt>
-</wpt>
-''' % (w.x, w.y, w.z, i))
-
-    def add_header():
-        outf.write('''<?xml version="1.0" encoding="UTF-8"?>
-<gpx
-  version="1.0"
-  creator="pymavlink"
-  xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
-  xmlns="http://www.topografix.com/GPX/1/0"
-  xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
-''')
-
-    def add_footer():
-        outf.write('''
-</gpx>
-''')
-
-    add_header()       
-
-    count = 0
-    for i in range(wp.count()):
-        w = wp.wp(i)
-        if w.frame == 3:
-            w.z += wp.wp(0).z
-        if w.command == 16:
-            process_wp(w, i)
-        count += 1
-    add_footer()
-    print("Created %s with %u points" % (outfilename, count))
-    
-
-for infilename in args:
-    outfilename = infilename + '.gpx'
-    wp_to_gpx(infilename, outfilename)
diff --git a/mavlink/share/pyshared/pymavlink/fgFDM.py b/mavlink/share/pyshared/pymavlink/fgFDM.py
deleted file mode 100644
index f390e0a93..000000000
--- a/mavlink/share/pyshared/pymavlink/fgFDM.py
+++ /dev/null
@@ -1,209 +0,0 @@
-#!/usr/bin/env python
-# parse and construct FlightGear NET FDM packets
-# Andrew Tridgell, November 2011
-# released under GNU GPL version 2 or later
-
-import struct, math
-
-class fgFDMError(Exception):
-    '''fgFDM error class'''
-    def __init__(self, msg):
-        Exception.__init__(self, msg)
-        self.message = 'fgFDMError: ' + msg
-
-class fgFDMVariable(object):
-    '''represent a single fgFDM variable'''
-    def __init__(self, index, arraylength, units):
-        self.index   = index
-        self.arraylength = arraylength
-        self.units = units
-
-class fgFDMVariableList(object):
-    '''represent a list of fgFDM variable'''
-    def __init__(self):
-        self.vars = {}
-        self._nextidx = 0
-        
-    def add(self, varname, arraylength=1, units=None):
-        self.vars[varname] = fgFDMVariable(self._nextidx, arraylength, units=units)
-        self._nextidx += arraylength
-
-class fgFDM(object):
-    '''a flightgear native FDM parser/generator'''
-    def __init__(self):
-        '''init a fgFDM object'''
-        self.FG_NET_FDM_VERSION = 24
-        self.pack_string = '>I 4x 3d 6f 11f 3f 2f I 4I 4f 4f 4f 4f 4f 4f 4f 4f 4f I 4f I 3I 3f 3f 3f I i f 10f'
-        self.values = [0]*98
-
-        self.FG_MAX_ENGINES = 4
-        self.FG_MAX_WHEELS  = 3
-        self.FG_MAX_TANKS   = 4
-
-        # supported unit mappings
-        self.unitmap = {
-            ('radians', 'degrees') : math.degrees(1),
-            ('rps',     'dps')     : math.degrees(1),
-            ('feet',    'meters')  : 0.3048,
-            ('fps',     'mps')     : 0.3048,
-            ('knots',   'mps')     : 0.514444444,
-            ('knots',   'fps')     : 0.514444444/0.3048,
-            ('fpss',    'mpss')    : 0.3048,
-            ('seconds', 'minutes') : 60,
-            ('seconds', 'hours')   : 3600,
-            }
-
-        # build a mapping between variable name and index in the values array
-        # note that the order of this initialisation is critical - it must
-        # match the wire structure
-        self.mapping = fgFDMVariableList()
-        self.mapping.add('version')
-
-        # position
-        self.mapping.add('longitude', units='radians')	# geodetic (radians)
-        self.mapping.add('latitude', units='radians')	# geodetic (radians)
-        self.mapping.add('altitude', units='meters')	# above sea level (meters)
-        self.mapping.add('agl', units='meters')		# above ground level (meters)
-
-        # attitude
-        self.mapping.add('phi', units='radians')	# roll (radians)
-        self.mapping.add('theta', units='radians')	# pitch (radians)
-        self.mapping.add('psi', units='radians')	# yaw or true heading (radians)
-        self.mapping.add('alpha', units='radians')      # angle of attack (radians)
-        self.mapping.add('beta', units='radians')       # side slip angle (radians)
-
-        # Velocities
-        self.mapping.add('phidot', units='rps')		# roll rate (radians/sec)
-        self.mapping.add('thetadot', units='rps')	# pitch rate (radians/sec)
-        self.mapping.add('psidot', units='rps')		# yaw rate (radians/sec)
-        self.mapping.add('vcas', units='fps')		# calibrated airspeed
-        self.mapping.add('climb_rate', units='fps')	# feet per second
-        self.mapping.add('v_north', units='fps')        # north velocity in local/body frame, fps
-        self.mapping.add('v_east', units='fps')         # east velocity in local/body frame, fps
-        self.mapping.add('v_down', units='fps')         # down/vertical velocity in local/body frame, fps
-        self.mapping.add('v_wind_body_north', units='fps')   # north velocity in local/body frame
-        self.mapping.add('v_wind_body_east', units='fps')    # east velocity in local/body frame
-        self.mapping.add('v_wind_body_down', units='fps')    # down/vertical velocity in local/body
-
-        # Accelerations
-        self.mapping.add('A_X_pilot', units='fpss')	# X accel in body frame ft/sec^2
-        self.mapping.add('A_Y_pilot', units='fpss')	# Y accel in body frame ft/sec^2
-        self.mapping.add('A_Z_pilot', units='fpss')	# Z accel in body frame ft/sec^2
-
-        # Stall
-        self.mapping.add('stall_warning')               # 0.0 - 1.0 indicating the amount of stall
-        self.mapping.add('slip_deg', units='degrees')	# slip ball deflection
-
-        # Engine status
-        self.mapping.add('num_engines')	                    # Number of valid engines
-        self.mapping.add('eng_state', self.FG_MAX_ENGINES)  # Engine state (off, cranking, running)
-        self.mapping.add('rpm',       self.FG_MAX_ENGINES)  # Engine RPM rev/min
-        self.mapping.add('fuel_flow', self.FG_MAX_ENGINES)  # Fuel flow gallons/hr
-        self.mapping.add('fuel_px',   self.FG_MAX_ENGINES)  # Fuel pressure psi
-        self.mapping.add('egt',       self.FG_MAX_ENGINES)  # Exhuast gas temp deg F
-        self.mapping.add('cht',       self.FG_MAX_ENGINES)  # Cylinder head temp deg F
-        self.mapping.add('mp_osi',    self.FG_MAX_ENGINES)  # Manifold pressure
-        self.mapping.add('tit',       self.FG_MAX_ENGINES)  # Turbine Inlet Temperature
-        self.mapping.add('oil_temp',  self.FG_MAX_ENGINES)  # Oil temp deg F
-        self.mapping.add('oil_px',    self.FG_MAX_ENGINES)  # Oil pressure psi
-            
-        # Consumables
-        self.mapping.add('num_tanks')		            # Max number of fuel tanks
-        self.mapping.add('fuel_quantity', self.FG_MAX_TANKS)
-
-        # Gear status
-        self.mapping.add('num_wheels')
-        self.mapping.add('wow',              self.FG_MAX_WHEELS)
-        self.mapping.add('gear_pos',         self.FG_MAX_WHEELS)
-        self.mapping.add('gear_steer',       self.FG_MAX_WHEELS)
-        self.mapping.add('gear_compression', self.FG_MAX_WHEELS)
-
-        # Environment
-        self.mapping.add('cur_time', units='seconds')       # current unix time
-        self.mapping.add('warp',     units='seconds')       # offset in seconds to unix time
-        self.mapping.add('visibility', units='meters')      # visibility in meters (for env. effects)
-
-        # Control surface positions (normalized values)
-        self.mapping.add('elevator')
-        self.mapping.add('elevator_trim_tab')
-        self.mapping.add('left_flap')
-        self.mapping.add('right_flap')
-        self.mapping.add('left_aileron')
-        self.mapping.add('right_aileron')
-        self.mapping.add('rudder')
-        self.mapping.add('nose_wheel')
-        self.mapping.add('speedbrake')
-        self.mapping.add('spoilers')
-
-        self._packet_size = struct.calcsize(self.pack_string)
-
-        self.set('version', self.FG_NET_FDM_VERSION)
-
-        if len(self.values) != self.mapping._nextidx:
-            raise fgFDMError('Invalid variable list in initialisation')
-
-    def packet_size(self):
-        '''return expected size of FG FDM packets'''
-        return self._packet_size
-
-    def convert(self, value, fromunits, tounits):
-        '''convert a value from one set of units to another'''
-        if fromunits == tounits:
-            return value
-        if (fromunits,tounits) in self.unitmap:
-            return value * self.unitmap[(fromunits,tounits)]
-        if (tounits,fromunits) in self.unitmap:
-            return value / self.unitmap[(tounits,fromunits)]
-        raise fgFDMError("unknown unit mapping (%s,%s)" % (fromunits, tounits))
-
-
-    def units(self, varname):
-        '''return the default units of a variable'''
-        if not varname in self.mapping.vars:
-            raise fgFDMError('Unknown variable %s' % varname)
-        return self.mapping.vars[varname].units
-
-
-    def variables(self):
-        '''return a list of available variables'''
-        return sorted(self.mapping.vars.keys(),
-                      key = lambda v : self.mapping.vars[v].index)
-
-
-    def get(self, varname, idx=0, units=None):
-        '''get a variable value'''
-        if not varname in self.mapping.vars:
-            raise fgFDMError('Unknown variable %s' % varname)
-        if idx >= self.mapping.vars[varname].arraylength:
-            raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % (
-                varname, idx, self.mapping.vars[varname].arraylength))
-        value = self.values[self.mapping.vars[varname].index + idx]
-        if units:
-            value = self.convert(value, self.mapping.vars[varname].units, units)
-        return value
-
-    def set(self, varname, value, idx=0, units=None):
-        '''set a variable value'''
-        if not varname in self.mapping.vars:
-            raise fgFDMError('Unknown variable %s' % varname)
-        if idx >= self.mapping.vars[varname].arraylength:
-            raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % (
-                varname, idx, self.mapping.vars[varname].arraylength))
-        if units:
-            value = self.convert(value, units, self.mapping.vars[varname].units)
-        self.values[self.mapping.vars[varname].index + idx] = value
-
-    def parse(self, buf):
-        '''parse a FD FDM buffer'''
-        try:
-            t = struct.unpack(self.pack_string, buf)
-        except struct.error, msg:
-            raise fgFDMError('unable to parse - %s' % msg)
-        self.values = list(t)
-
-    def pack(self):
-        '''pack a FD FDM buffer from current values'''
-        for i in range(len(self.values)):
-            if math.isnan(self.values[i]):
-                self.values[i] = 0
-        return struct.pack(self.pack_string, *self.values)
diff --git a/mavlink/share/pyshared/pymavlink/generator/.gitignore b/mavlink/share/pyshared/pymavlink/generator/.gitignore
deleted file mode 100644
index 0d20b6487..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-*.pyc
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h
deleted file mode 100644
index b70991f5a..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h
+++ /dev/null
@@ -1,89 +0,0 @@
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef _CHECKSUM_H_
-#define _CHECKSUM_H_
-
-
-/**
- *
- *  CALCULATE THE CHECKSUM
- *
- */
-
-#define X25_INIT_CRC 0xffff
-#define X25_VALIDATE_CRC 0xf0b8
-
-/**
- * @brief Accumulate the X.25 CRC by adding one char at a time.
- *
- * The checksum function adds the hash of one char at a time to the
- * 16 bit checksum (uint16_t).
- *
- * @param data new char to hash
- * @param crcAccum the already accumulated checksum
- **/
-static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
-{
-        /*Accumulate one byte of data into the CRC*/
-        uint8_t tmp;
-
-        tmp = data ^ (uint8_t)(*crcAccum &0xff);
-        tmp ^= (tmp<<4);
-        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
-}
-
-/**
- * @brief Initiliaze the buffer for the X.25 CRC
- *
- * @param crcAccum the 16 bit X.25 CRC
- */
-static inline void crc_init(uint16_t* crcAccum)
-{
-        *crcAccum = X25_INIT_CRC;
-}
-
-
-/**
- * @brief Calculates the X.25 checksum on a byte buffer
- *
- * @param  pBuffer buffer containing the byte array to hash
- * @param  length  length of the byte array
- * @return the checksum over the buffer bytes
- **/
-static inline uint16_t crc_calculate(uint8_t* pBuffer, uint16_t length)
-{
-        uint16_t crcTmp;
-        crc_init(&crcTmp);
-	while (length--) {
-                crc_accumulate(*pBuffer++, &crcTmp);
-        }
-        return crcTmp;
-}
-
-/**
- * @brief Accumulate the X.25 CRC by adding an array of bytes
- *
- * The checksum function adds the hash of one char at a time to the
- * 16 bit checksum (uint16_t).
- *
- * @param data new bytes to hash
- * @param crcAccum the already accumulated checksum
- **/
-static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
-{
-	const uint8_t *p = (const uint8_t *)pBuffer;
-	while (length--) {
-                crc_accumulate(*p++, crcAccum);
-        }
-}
-
-
-
-
-#endif /* _CHECKSUM_H_ */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_helpers.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_helpers.h
deleted file mode 100644
index 98250e1ac..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_helpers.h
+++ /dev/null
@@ -1,488 +0,0 @@
-#ifndef  _MAVLINK_HELPERS_H_
-#define  _MAVLINK_HELPERS_H_
-
-#include "string.h"
-#include "checksum.h"
-#include "mavlink_types.h"
-
-#ifndef MAVLINK_HELPER
-#define MAVLINK_HELPER
-#endif
-
-/*
-  internal function to give access to the channel status for each channel
- */
-MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
-{
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[chan];
-}
-
-/**
- * @brief Finalize a MAVLink message with channel assignment
- *
- * This function calculates the checksum and sets length and aircraft id correctly.
- * It assumes that the message id and the payload are already correctly set. This function
- * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
- * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
- *
- * @param msg Message to finalize
- * @param system_id Id of the sending (this) system, 1-127
- * @param length Message length
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length, uint8_t crc_extra)
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length)
-#endif
-{
-	// This code part is the same for all messages;
-	uint16_t checksum;
-	msg->magic = MAVLINK_STX;
-	msg->len = length;
-	msg->sysid = system_id;
-	msg->compid = component_id;
-	// One sequence number per component
-	msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
-	mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
-	checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
-#if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
-#endif
-	mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
-	mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
-
-	return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-}
-
-
-/**
- * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length, uint8_t crc_extra)
-{
-	return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
-}
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length)
-{
-	return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
-}
-#endif
-
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
-
-/**
- * @brief Finalize a MAVLink message with channel assignment and send
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
-						    uint8_t length, uint8_t crc_extra)
-#else
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
-#endif
-{
-	uint16_t checksum;
-	uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
-	uint8_t ck[2];
-	mavlink_status_t *status = mavlink_get_channel_status(chan);
-	buf[0] = MAVLINK_STX;
-	buf[1] = length;
-	buf[2] = status->current_tx_seq;
-	buf[3] = mavlink_system.sysid;
-	buf[4] = mavlink_system.compid;
-	buf[5] = msgid;
-	status->current_tx_seq++;
-	checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
-	crc_accumulate_buffer(&checksum, packet, length);
-#if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
-#endif
-	ck[0] = (uint8_t)(checksum & 0xFF);
-	ck[1] = (uint8_t)(checksum >> 8);
-
-	MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
-	_mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
-	_mavlink_send_uart(chan, packet, length);
-	_mavlink_send_uart(chan, (const char *)ck, 2);
-	MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
-}
-#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-/**
- * @brief Pack a message to send it over a serial byte stream
- */
-MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
-{
-	memcpy(buffer, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
-	return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
-}
-
-union __mavlink_bitfield {
-	uint8_t uint8;
-	int8_t int8;
-	uint16_t uint16;
-	int16_t int16;
-	uint32_t uint32;
-	int32_t int32;
-};
-
-
-MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
-{
-	crc_init(&msg->checksum);
-}
-
-MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
-{
-	crc_accumulate(c, &msg->checksum);
-}
-
-/**
- * This is a convenience function which handles the complete MAVLink parsing.
- * the function will parse one byte at a time and return the complete packet once
- * it could be successfully decoded. Checksum and other failures will be silently
- * ignored.
- *
- * @param chan     ID of the current channel. This allows to parse different channels with this function.
- *                 a channel is not a physical message channel like a serial port, but a logic partition of
- *                 the communication streams in this case. COMM_NB is the limit for the number of channels
- *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
- * @param c        The char to barse
- *
- * @param returnMsg NULL if no message could be decoded, the message data else
- * @return 0 if no message could be decoded, 1 else
- *
- * A typical use scenario of this function call is:
- *
- * @code
- * #include <inttypes.h> // For fixed-width uint8_t type
- *
- * mavlink_message_t msg;
- * int chan = 0;
- *
- *
- * while(serial.bytesAvailable > 0)
- * {
- *   uint8_t byte = serial.getNextByte();
- *   if (mavlink_parse_char(chan, byte, &msg))
- *     {
- *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
- *     }
- * }
- *
- *
- * @endcode
- */
-MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
-{
-	static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
-
-        /*
-	  default message crc function. You can override this per-system to
-	  put this data in a different memory segment
-	*/
-#if MAVLINK_CRC_EXTRA
-#ifndef MAVLINK_MESSAGE_CRC
-	static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
-#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
-#endif
-#endif
-
-	mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message
-	mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
-	int bufferIndex = 0;
-
-	status->msg_received = 0;
-
-	switch (status->parse_state)
-	{
-	case MAVLINK_PARSE_STATE_UNINIT:
-	case MAVLINK_PARSE_STATE_IDLE:
-		if (c == MAVLINK_STX)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-			rxmsg->len = 0;
-			mavlink_start_checksum(rxmsg);
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_STX:
-			if (status->msg_received 
-			/* Support shorter buffers than the
-			 default maximum packet size */
-#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
-				|| c > MAVLINK_MAX_PAYLOAD_LEN
-#endif
-				)
-		{
-			status->buffer_overrun++;
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-		}
-		else
-		{
-			// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
-			rxmsg->len = c;
-			status->packet_idx = 0;
-			mavlink_update_checksum(rxmsg, c);
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_LENGTH:
-		rxmsg->seq = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_SEQ:
-		rxmsg->sysid = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_SYSID:
-		rxmsg->compid = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_COMPID:
-		rxmsg->msgid = c;
-		mavlink_update_checksum(rxmsg, c);
-		if (rxmsg->len == 0)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
-		}
-		else
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_MSGID:
-		_MAV_PAYLOAD(rxmsg)[status->packet_idx++] = (char)c;
-		mavlink_update_checksum(rxmsg, c);
-		if (status->packet_idx == rxmsg->len)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
-#if MAVLINK_CRC_EXTRA
-		mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
-#endif
-		if (c != (rxmsg->checksum & 0xFF)) {
-			// Check first checksum byte
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			if (c == MAVLINK_STX)
-			{
-				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-				rxmsg->len = 0;
-				mavlink_start_checksum(rxmsg);
-			}
-		}
-		else
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_CRC1:
-		if (c != (rxmsg->checksum >> 8)) {
-			// Check second checksum byte
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			if (c == MAVLINK_STX)
-			{
-				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-				rxmsg->len = 0;
-				mavlink_start_checksum(rxmsg);
-			}
-		}
-		else
-		{
-			// Successfully got message
-			status->msg_received = 1;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
-		}
-		break;
-	}
-
-	bufferIndex++;
-	// If a message has been sucessfully decoded, check index
-	if (status->msg_received == 1)
-	{
-		//while(status->current_seq != rxmsg->seq)
-		//{
-		//	status->packet_rx_drop_count++;
-		//               status->current_seq++;
-		//}
-		status->current_rx_seq = rxmsg->seq;
-		// Initial condition: If no packet has been received so far, drop count is undefined
-		if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
-		// Count this packet as received
-		status->packet_rx_success_count++;
-	}
-
-	r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
-	r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
-	r_mavlink_status->packet_rx_drop_count = status->parse_error;
-	status->parse_error = 0;
-	return status->msg_received;
-}
-
-/**
- * @brief Put a bitfield of length 1-32 bit into the buffer
- *
- * @param b the value to add, will be encoded in the bitfield
- * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
- * @param packet_index the position in the packet (the index of the first byte to use)
- * @param bit_index the position in the byte (the index of the first bit to use)
- * @param buffer packet buffer to write into
- * @return new position of the last used byte in the buffer
- */
-MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
-{
-	uint16_t bits_remain = bits;
-	// Transform number into network order
-	int32_t v;
-	uint8_t i_bit_index, i_byte_index, curr_bits_n;
-#if MAVLINK_NEED_BYTE_SWAP
-	union {
-		int32_t i;
-		uint8_t b[4];
-	} bin, bout;
-	bin.i = b;
-	bout.b[0] = bin.b[3];
-	bout.b[1] = bin.b[2];
-	bout.b[2] = bin.b[1];
-	bout.b[3] = bin.b[0];
-	v = bout.i;
-#else
-	v = b;
-#endif
-
-	// buffer in
-	// 01100000 01000000 00000000 11110001
-	// buffer out
-	// 11110001 00000000 01000000 01100000
-
-	// Existing partly filled byte (four free slots)
-	// 0111xxxx
-
-	// Mask n free bits
-	// 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
-	// = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
-
-	// Shift n bits into the right position
-	// out = in >> n;
-
-	// Mask and shift bytes
-	i_bit_index = bit_index;
-	i_byte_index = packet_index;
-	if (bit_index > 0)
-	{
-		// If bits were available at start, they were available
-		// in the byte before the current index
-		i_byte_index--;
-	}
-
-	// While bits have not been packed yet
-	while (bits_remain > 0)
-	{
-		// Bits still have to be packed
-		// there can be more than 8 bits, so
-		// we might have to pack them into more than one byte
-
-		// First pack everything we can into the current 'open' byte
-		//curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
-		//FIXME
-		if (bits_remain <= (uint8_t)(8 - i_bit_index))
-		{
-			// Enough space
-			curr_bits_n = (uint8_t)bits_remain;
-		}
-		else
-		{
-			curr_bits_n = (8 - i_bit_index);
-		}
-		
-		// Pack these n bits into the current byte
-		// Mask out whatever was at that position with ones (xxx11111)
-		buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
-		// Put content to this position, by masking out the non-used part
-		buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
-		
-		// Increment the bit index
-		i_bit_index += curr_bits_n;
-
-		// Now proceed to the next byte, if necessary
-		bits_remain -= curr_bits_n;
-		if (bits_remain > 0)
-		{
-			// Offer another 8 bits / one byte
-			i_byte_index++;
-			i_bit_index = 0;
-		}
-	}
-	
-	*r_bit_index = i_bit_index;
-	// If a partly filled byte is present, mark this as consumed
-	if (i_bit_index != 7) i_byte_index++;
-	return i_byte_index - packet_index;
-}
-
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-// To make MAVLink work on your MCU, define comm_send_ch() if you wish
-// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
-// whole packet at a time
-
-/*
-
-#include "mavlink_types.h"
-
-void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
-{
-    if (chan == MAVLINK_COMM_0)
-    {
-        uart0_transmit(ch);
-    }
-    if (chan == MAVLINK_COMM_1)
-    {
-    	uart1_transmit(ch);
-    }
-}
- */
-
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
-{
-#ifdef MAVLINK_SEND_UART_BYTES
-	/* this is the more efficient approach, if the platform
-	   defines it */
-	MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len);
-#else
-	/* fallback to one byte at a time */
-	uint16_t i;
-	for (i = 0; i < len; i++) {
-		comm_send_ch(chan, (uint8_t)buf[i]);
-	}
-#endif
-}
-#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-#endif /* _MAVLINK_HELPERS_H_ */
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_types.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_types.h
deleted file mode 100644
index 630cb84b7..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_types.h
+++ /dev/null
@@ -1,300 +0,0 @@
-#ifndef MAVLINK_TYPES_H_
-#define MAVLINK_TYPES_H_
-
-#include "inttypes.h"
-
-enum MAV_CLASS
-{
-    MAV_CLASS_GENERIC = 0,        ///< Generic autopilot, full support for everything
-    MAV_CLASS_PIXHAWK = 1,        ///< PIXHAWK autopilot, http://pixhawk.ethz.ch
-    MAV_CLASS_SLUGS = 2,          ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu
-    MAV_CLASS_ARDUPILOTMEGA = 3,  ///< ArduPilotMega / ArduCopter, http://diydrones.com
-    MAV_CLASS_OPENPILOT = 4,      ///< OpenPilot, http://openpilot.org
-    MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5,  ///< Generic autopilot only supporting simple waypoints
-    MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands
-    MAV_CLASS_GENERIC_MISSION_FULL = 7,            ///< Generic autopilot supporting the full mission command set
-    MAV_CLASS_NONE = 8,           ///< No valid autopilot
-    MAV_CLASS_NB                  ///< Number of autopilot classes
-};
-
-enum MAV_ACTION
-{
-    MAV_ACTION_HOLD = 0,
-    MAV_ACTION_MOTORS_START = 1,
-    MAV_ACTION_LAUNCH = 2,
-    MAV_ACTION_RETURN = 3,
-    MAV_ACTION_EMCY_LAND = 4,
-    MAV_ACTION_EMCY_KILL = 5,
-    MAV_ACTION_CONFIRM_KILL = 6,
-    MAV_ACTION_CONTINUE = 7,
-    MAV_ACTION_MOTORS_STOP = 8,
-    MAV_ACTION_HALT = 9,
-    MAV_ACTION_SHUTDOWN = 10,
-    MAV_ACTION_REBOOT = 11,
-    MAV_ACTION_SET_MANUAL = 12,
-    MAV_ACTION_SET_AUTO = 13,
-    MAV_ACTION_STORAGE_READ = 14,
-    MAV_ACTION_STORAGE_WRITE = 15,
-    MAV_ACTION_CALIBRATE_RC = 16,
-    MAV_ACTION_CALIBRATE_GYRO = 17,
-    MAV_ACTION_CALIBRATE_MAG = 18,
-    MAV_ACTION_CALIBRATE_ACC = 19,
-    MAV_ACTION_CALIBRATE_PRESSURE = 20,
-    MAV_ACTION_REC_START = 21,
-    MAV_ACTION_REC_PAUSE = 22,
-    MAV_ACTION_REC_STOP = 23,
-    MAV_ACTION_TAKEOFF = 24,
-    MAV_ACTION_NAVIGATE = 25,
-    MAV_ACTION_LAND = 26,
-    MAV_ACTION_LOITER = 27,
-    MAV_ACTION_SET_ORIGIN = 28,
-    MAV_ACTION_RELAY_ON = 29,
-    MAV_ACTION_RELAY_OFF = 30,
-    MAV_ACTION_GET_IMAGE = 31,
-    MAV_ACTION_VIDEO_START = 32,
-    MAV_ACTION_VIDEO_STOP = 33,
-    MAV_ACTION_RESET_MAP = 34,
-    MAV_ACTION_RESET_PLAN = 35,
-    MAV_ACTION_DELAY_BEFORE_COMMAND = 36,
-    MAV_ACTION_ASCEND_AT_RATE = 37,
-    MAV_ACTION_CHANGE_MODE = 38,
-    MAV_ACTION_LOITER_MAX_TURNS = 39,
-    MAV_ACTION_LOITER_MAX_TIME = 40,
-    MAV_ACTION_START_HILSIM = 41,
-    MAV_ACTION_STOP_HILSIM = 42,    
-    MAV_ACTION_NB        ///< Number of MAV actions
-};
-
-enum MAV_MODE
-{
-    MAV_MODE_UNINIT = 0,     ///< System is in undefined state
-    MAV_MODE_LOCKED = 1,     ///< Motors are blocked, system is safe
-    MAV_MODE_MANUAL = 2,     ///< System is allowed to be active, under manual (RC) control
-    MAV_MODE_GUIDED = 3,     ///< System is allowed to be active, under autonomous control, manual setpoint
-    MAV_MODE_AUTO =   4,     ///< System is allowed to be active, under autonomous control and navigation
-    MAV_MODE_TEST1 =  5,     ///< Generic test mode, for custom use
-    MAV_MODE_TEST2 =  6,     ///< Generic test mode, for custom use
-    MAV_MODE_TEST3 =  7,     ///< Generic test mode, for custom use
-    MAV_MODE_READY =  8,     ///< System is ready, motors are unblocked, but controllers are inactive
-    MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back
-};
-
-enum MAV_STATE
-{
-    MAV_STATE_UNINIT = 0,
-    MAV_STATE_BOOT,
-    MAV_STATE_CALIBRATING,
-    MAV_STATE_STANDBY,
-    MAV_STATE_ACTIVE,
-    MAV_STATE_CRITICAL,
-    MAV_STATE_EMERGENCY,
-    MAV_STATE_HILSIM,
-    MAV_STATE_POWEROFF
-};
-
-enum MAV_NAV
-{
-    MAV_NAV_GROUNDED = 0,
-    MAV_NAV_LIFTOFF,
-    MAV_NAV_HOLD,
-    MAV_NAV_WAYPOINT,
-    MAV_NAV_VECTOR,
-    MAV_NAV_RETURNING,
-    MAV_NAV_LANDING,
-    MAV_NAV_LOST,
-    MAV_NAV_LOITER,
-    MAV_NAV_FREE_DRIFT
-};
-
-enum MAV_TYPE
-{
-    MAV_GENERIC = 0,
-    MAV_FIXED_WING = 1,
-    MAV_QUADROTOR = 2,
-    MAV_COAXIAL = 3,
-    MAV_HELICOPTER = 4,
-    MAV_GROUND = 5,
-    OCU = 6,
-    MAV_AIRSHIP = 7,
-    MAV_FREE_BALLOON = 8,
-    MAV_ROCKET = 9,
-    UGV_GROUND_ROVER = 10,
-    UGV_SURFACE_SHIP = 11
-};
-
-enum MAV_AUTOPILOT_TYPE
-{
-    MAV_AUTOPILOT_GENERIC = 0,
-    MAV_AUTOPILOT_PIXHAWK = 1,
-    MAV_AUTOPILOT_SLUGS = 2,
-    MAV_AUTOPILOT_ARDUPILOTMEGA = 3,
-    MAV_AUTOPILOT_NONE = 4
-};
-
-enum MAV_COMPONENT
-{
-    MAV_COMP_ID_GPS,
-    MAV_COMP_ID_WAYPOINTPLANNER,
-    MAV_COMP_ID_BLOBTRACKER,
-    MAV_COMP_ID_PATHPLANNER,
-    MAV_COMP_ID_AIRSLAM,
-    MAV_COMP_ID_MAPPER,
-    MAV_COMP_ID_CAMERA,
-    MAV_COMP_ID_RADIO = 68,
-    MAV_COMP_ID_IMU = 200,
-    MAV_COMP_ID_IMU_2 = 201,
-    MAV_COMP_ID_IMU_3 = 202,
-    MAV_COMP_ID_UDP_BRIDGE = 240,
-    MAV_COMP_ID_UART_BRIDGE = 241,
-    MAV_COMP_ID_SYSTEM_CONTROL = 250
-};
-
-enum MAV_FRAME
-{
-    MAV_FRAME_GLOBAL = 0,
-    MAV_FRAME_LOCAL = 1,
-    MAV_FRAME_MISSION = 2,
-    MAV_FRAME_GLOBAL_RELATIVE_ALT = 3,
-    MAV_FRAME_LOCAL_ENU = 4
-};
-
-enum MAVLINK_DATA_STREAM_TYPE
-{
-    MAVLINK_DATA_STREAM_IMG_JPEG,
-    MAVLINK_DATA_STREAM_IMG_BMP,
-    MAVLINK_DATA_STREAM_IMG_RAW8U,
-    MAVLINK_DATA_STREAM_IMG_RAW32U,
-    MAVLINK_DATA_STREAM_IMG_PGM,
-    MAVLINK_DATA_STREAM_IMG_PNG
-};
-
-#ifndef MAVLINK_MAX_PAYLOAD_LEN
-// it is possible to override this, but be careful!
-#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
-#endif
-
-#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
-#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum
-#define MAVLINK_NUM_CHECKSUM_BYTES 2
-#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
-
-#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
-
-typedef struct param_union {
-	union {
-    float param_float;
-    int32_t param_int32;
-    uint32_t param_uint32;
-	};
-	uint8_t type;
-} mavlink_param_union_t;
-
-typedef struct __mavlink_system {
-    uint8_t sysid;   ///< Used by the MAVLink message_xx_send() convenience function
-    uint8_t compid;  ///< Used by the MAVLink message_xx_send() convenience function
-    uint8_t type;    ///< Unused, can be used by user to store the system's type
-    uint8_t state;   ///< Unused, can be used by user to store the system's state
-    uint8_t mode;    ///< Unused, can be used by user to store the system's mode
-    uint8_t nav_mode;    ///< Unused, can be used by user to store the system's navigation mode
-} mavlink_system_t;
-
-typedef struct __mavlink_message {
-	uint16_t checksum; /// sent at end of packet
-	uint8_t magic;   ///< protocol magic marker
-	uint8_t len;     ///< Length of payload
-	uint8_t seq;     ///< Sequence of packet
-	uint8_t sysid;   ///< ID of message sender system/aircraft
-	uint8_t compid;  ///< ID of the message sender component
-	uint8_t msgid;   ///< ID of message in payload
-	uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
-} mavlink_message_t;
-
-typedef enum {
-	MAVLINK_TYPE_CHAR     = 0,
-	MAVLINK_TYPE_UINT8_T  = 1,
-	MAVLINK_TYPE_INT8_T   = 2,
-	MAVLINK_TYPE_UINT16_T = 3,
-	MAVLINK_TYPE_INT16_T  = 4,
-	MAVLINK_TYPE_UINT32_T = 5,
-	MAVLINK_TYPE_INT32_T  = 6,
-	MAVLINK_TYPE_UINT64_T = 7,
-	MAVLINK_TYPE_INT64_T  = 8,
-	MAVLINK_TYPE_FLOAT    = 9,
-	MAVLINK_TYPE_DOUBLE   = 10
-} mavlink_message_type_t;
-
-#define MAVLINK_MAX_FIELDS 64
-
-typedef struct __mavlink_field_info {
-	const char *name;             // name of this field
-	const char *print_format;     // printing format hint, or NULL
-	mavlink_message_type_t type;  // type of this field
-	unsigned array_length;        // if non-zero, field is an array
-	unsigned wire_offset;         // offset of each field in the payload
-	unsigned structure_offset;    // offset in a C structure
-} mavlink_field_info_t;
-
-// note that in this structure the order of fields is the order
-// in the XML file, not necessary the wire order
-typedef struct __mavlink_message_info {
-	const char *name;                                      // name of the message
-	unsigned num_fields;                                   // how many fields in this message
-	mavlink_field_info_t fields[MAVLINK_MAX_FIELDS];       // field information
-} mavlink_message_info_t;
-
-#define _MAV_PAYLOAD(msg) ((char *)(&(msg)->payload64[0]))
-#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0])))
-
-// checksum is immediately after the payload bytes
-#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD(msg))
-#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD(msg))
-
-typedef enum {
-    MAVLINK_COMM_0,
-    MAVLINK_COMM_1,
-    MAVLINK_COMM_2,
-    MAVLINK_COMM_3
-} mavlink_channel_t;
-
-/*
- * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
- * of buffers they will use. If more are used, then the result will be
- * a stack overrun
- */
-#ifndef MAVLINK_COMM_NUM_BUFFERS
-#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
-# define MAVLINK_COMM_NUM_BUFFERS 16
-#else
-# define MAVLINK_COMM_NUM_BUFFERS 4
-#endif
-#endif
-
-typedef enum {
-    MAVLINK_PARSE_STATE_UNINIT=0,
-    MAVLINK_PARSE_STATE_IDLE,
-    MAVLINK_PARSE_STATE_GOT_STX,
-    MAVLINK_PARSE_STATE_GOT_SEQ,
-    MAVLINK_PARSE_STATE_GOT_LENGTH,
-    MAVLINK_PARSE_STATE_GOT_SYSID,
-    MAVLINK_PARSE_STATE_GOT_COMPID,
-    MAVLINK_PARSE_STATE_GOT_MSGID,
-    MAVLINK_PARSE_STATE_GOT_PAYLOAD,
-    MAVLINK_PARSE_STATE_GOT_CRC1
-} mavlink_parse_state_t; ///< The state machine for the comm parser
-
-typedef struct __mavlink_status {
-    uint8_t msg_received;               ///< Number of received messages
-    uint8_t buffer_overrun;             ///< Number of buffer overruns
-    uint8_t parse_error;                ///< Number of parse errors
-    mavlink_parse_state_t parse_state;  ///< Parsing state machine
-    uint8_t packet_idx;                 ///< Index in current packet
-    uint8_t current_rx_seq;             ///< Sequence number of last packet received
-    uint8_t current_tx_seq;             ///< Sequence number of last packet sent
-    uint16_t packet_rx_success_count;   ///< Received packets
-    uint16_t packet_rx_drop_count;      ///< Number of packet drops
-} mavlink_status_t;
-
-#define MAVLINK_BIG_ENDIAN 0
-#define MAVLINK_LITTLE_ENDIAN 1
-
-#endif /* MAVLINK_TYPES_H_ */
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/protocol.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/protocol.h
deleted file mode 100644
index 05195c369..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/protocol.h
+++ /dev/null
@@ -1,319 +0,0 @@
-#ifndef  _MAVLINK_PROTOCOL_H_
-#define  _MAVLINK_PROTOCOL_H_
-
-#include "string.h"
-#include "mavlink_types.h"
-
-/* 
-   If you want MAVLink on a system that is native big-endian,
-   you need to define NATIVE_BIG_ENDIAN
-*/
-#ifdef NATIVE_BIG_ENDIAN
-# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
-#else
-# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
-#endif
-
-#ifndef MAVLINK_STACK_BUFFER
-#define MAVLINK_STACK_BUFFER 0
-#endif
-
-#ifndef MAVLINK_AVOID_GCC_STACK_BUG
-# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
-#endif
-
-#ifndef MAVLINK_ASSERT
-#define MAVLINK_ASSERT(x)
-#endif
-
-#ifndef MAVLINK_START_UART_SEND
-#define MAVLINK_START_UART_SEND(chan, length)
-#endif
-
-#ifndef MAVLINK_END_UART_SEND
-#define MAVLINK_END_UART_SEND(chan, length)
-#endif
-
-#ifdef MAVLINK_SEPARATE_HELPERS
-#define MAVLINK_HELPER
-#else
-#define MAVLINK_HELPER static inline
-#include "mavlink_helpers.h"
-#endif // MAVLINK_SEPARATE_HELPERS
-
-/* always include the prototypes to ensure we don't get out of sync */
-MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length, uint8_t crc_extra);
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length, uint8_t crc_extra);
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
-						    uint8_t length, uint8_t crc_extra);
-#endif
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length);
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length);
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
-#endif // MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
-MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
-MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
-MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
-MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, 
-					       uint8_t* r_bit_index, uint8_t* buffer);
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
-#endif
-
-/**
- * @brief Get the required buffer size for this message
- */
-static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
-{
-	return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-static inline void byte_swap_2(char *dst, const char *src)
-{
-	dst[0] = src[1];
-	dst[1] = src[0];
-}
-static inline void byte_swap_4(char *dst, const char *src)
-{
-	dst[0] = src[3];
-	dst[1] = src[2];
-	dst[2] = src[1];
-	dst[3] = src[0];
-}
-static inline void byte_swap_8(char *dst, const char *src)
-{
-	dst[0] = src[7];
-	dst[1] = src[6];
-	dst[2] = src[5];
-	dst[3] = src[4];
-	dst[4] = src[3];
-	dst[5] = src[2];
-	dst[6] = src[1];
-	dst[7] = src[0];
-}
-#elif !MAVLINK_ALIGNED_FIELDS
-static inline void byte_copy_2(char *dst, const char *src)
-{
-	dst[0] = src[0];
-	dst[1] = src[1];
-}
-static inline void byte_copy_4(char *dst, const char *src)
-{
-	dst[0] = src[0];
-	dst[1] = src[1];
-	dst[2] = src[2];
-	dst[3] = src[3];
-}
-static inline void byte_copy_8(char *dst, const char *src)
-{
-	memcpy(dst, src, 8);
-}
-#endif
-
-#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
-#define _mav_put_int8_t(buf, wire_offset, b)  buf[wire_offset] = (int8_t)b
-#define _mav_put_char(buf, wire_offset, b)    buf[wire_offset] = b
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int16_t(buf, wire_offset, b)  byte_swap_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int32_t(buf, wire_offset, b)  byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int64_t(buf, wire_offset, b)  byte_swap_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_float(buf, wire_offset, b)    byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_double(buf, wire_offset, b)   byte_swap_8(&buf[wire_offset], (const char *)&b)
-#elif !MAVLINK_ALIGNED_FIELDS
-#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int16_t(buf, wire_offset, b)  byte_copy_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int32_t(buf, wire_offset, b)  byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int64_t(buf, wire_offset, b)  byte_copy_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_float(buf, wire_offset, b)    byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_double(buf, wire_offset, b)   byte_copy_8(&buf[wire_offset], (const char *)&b)
-#else
-#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
-#define _mav_put_int16_t(buf, wire_offset, b)  *(int16_t *)&buf[wire_offset] = b
-#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
-#define _mav_put_int32_t(buf, wire_offset, b)  *(int32_t *)&buf[wire_offset] = b
-#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
-#define _mav_put_int64_t(buf, wire_offset, b)  *(int64_t *)&buf[wire_offset] = b
-#define _mav_put_float(buf, wire_offset, b)    *(float *)&buf[wire_offset] = b
-#define _mav_put_double(buf, wire_offset, b)   *(double *)&buf[wire_offset] = b
-#endif
-
-/*
-  like memcpy(), but if src is NULL, do a memset to zero
- */
-static void mav_array_memcpy(void *dest, const void *src, size_t n)
-{
-	if (src == NULL) {
-		memset(dest, 0, n);
-	} else {
-		memcpy(dest, src, n);
-	}
-}
-
-/*
- * Place a char array into a buffer
- */
-static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-}
-
-/*
- * Place a uint8_t array into a buffer
- */
-static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-}
-
-/*
- * Place a int8_t array into a buffer
- */
-static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_PUT_ARRAY(TYPE, V) \
-static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
-{ \
-	if (b == NULL) { \
-		memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
-	} else { \
-		uint16_t i; \
-		for (i=0; i<array_length; i++) { \
-			_mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
-		} \
-	} \
-}
-#else
-#define _MAV_PUT_ARRAY(TYPE, V)					\
-static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
-{ \
-	mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE));	\
-}
-#endif
-
-_MAV_PUT_ARRAY(uint16_t, u16)
-_MAV_PUT_ARRAY(uint32_t, u32)
-_MAV_PUT_ARRAY(uint64_t, u64)
-_MAV_PUT_ARRAY(int16_t,  i16)
-_MAV_PUT_ARRAY(int32_t,  i32)
-_MAV_PUT_ARRAY(int64_t,  i64)
-_MAV_PUT_ARRAY(float,    f)
-_MAV_PUT_ARRAY(double,   d)
-
-#define _MAV_RETURN_char(msg, wire_offset)             _MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_int8_t(msg, wire_offset)   (int8_t)_MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
-
-_MAV_MSG_RETURN_TYPE(uint16_t, 2)
-_MAV_MSG_RETURN_TYPE(int16_t,  2)
-_MAV_MSG_RETURN_TYPE(uint32_t, 4)
-_MAV_MSG_RETURN_TYPE(int32_t,  4)
-_MAV_MSG_RETURN_TYPE(uint64_t, 8)
-_MAV_MSG_RETURN_TYPE(int64_t,  8)
-_MAV_MSG_RETURN_TYPE(float,    4)
-_MAV_MSG_RETURN_TYPE(double,   8)
-
-#elif !MAVLINK_ALIGNED_FIELDS
-#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
-
-_MAV_MSG_RETURN_TYPE(uint16_t, 2)
-_MAV_MSG_RETURN_TYPE(int16_t,  2)
-_MAV_MSG_RETURN_TYPE(uint32_t, 4)
-_MAV_MSG_RETURN_TYPE(int32_t,  4)
-_MAV_MSG_RETURN_TYPE(uint64_t, 8)
-_MAV_MSG_RETURN_TYPE(int64_t,  8)
-_MAV_MSG_RETURN_TYPE(float,    4)
-_MAV_MSG_RETURN_TYPE(double,   8)
-#else // nicely aligned, no swap
-#define _MAV_MSG_RETURN_TYPE(TYPE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
-
-_MAV_MSG_RETURN_TYPE(uint16_t)
-_MAV_MSG_RETURN_TYPE(int16_t)
-_MAV_MSG_RETURN_TYPE(uint32_t)
-_MAV_MSG_RETURN_TYPE(int32_t)
-_MAV_MSG_RETURN_TYPE(uint64_t)
-_MAV_MSG_RETURN_TYPE(int64_t)
-_MAV_MSG_RETURN_TYPE(float)
-_MAV_MSG_RETURN_TYPE(double)
-#endif // MAVLINK_NEED_BYTE_SWAP
-
-static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, 
-						     uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, 
-							uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, 
-						       uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_RETURN_ARRAY(TYPE, V) \
-static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
-							 uint8_t array_length, uint8_t wire_offset) \
-{ \
-	uint16_t i; \
-	for (i=0; i<array_length; i++) { \
-		value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
-	} \
-	return array_length*sizeof(value[0]); \
-}
-#else
-#define _MAV_RETURN_ARRAY(TYPE, V)					\
-static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
-							 uint8_t array_length, uint8_t wire_offset) \
-{ \
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
-	return array_length*sizeof(TYPE); \
-}
-#endif
-
-_MAV_RETURN_ARRAY(uint16_t, u16)
-_MAV_RETURN_ARRAY(uint32_t, u32)
-_MAV_RETURN_ARRAY(uint64_t, u64)
-_MAV_RETURN_ARRAY(int16_t,  i16)
-_MAV_RETURN_ARRAY(int32_t,  i32)
-_MAV_RETURN_ARRAY(int64_t,  i64)
-_MAV_RETURN_ARRAY(float,    f)
-_MAV_RETURN_ARRAY(double,   d)
-
-#endif // _MAVLINK_PROTOCOL_H_
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink.h
deleted file mode 100644
index 5b91bfe2d..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from test.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 85
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_BIG_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 0
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 0
-#endif
-
-#include "version.h"
-#include "test.h"
-
-#endif // MAVLINK_H
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink_msg_test_types.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink_msg_test_types.h
deleted file mode 100644
index 50b4fd6bc..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink_msg_test_types.h
+++ /dev/null
@@ -1,610 +0,0 @@
-// MESSAGE TEST_TYPES PACKING
-
-#define MAVLINK_MSG_ID_TEST_TYPES 0
-
-typedef struct __mavlink_test_types_t
-{
- char c; ///< char
- char s[10]; ///< string
- uint8_t u8; ///< uint8_t
- uint16_t u16; ///< uint16_t
- uint32_t u32; ///< uint32_t
- uint64_t u64; ///< uint64_t
- int8_t s8; ///< int8_t
- int16_t s16; ///< int16_t
- int32_t s32; ///< int32_t
- int64_t s64; ///< int64_t
- float f; ///< float
- double d; ///< double
- uint8_t u8_array[3]; ///< uint8_t_array
- uint16_t u16_array[3]; ///< uint16_t_array
- uint32_t u32_array[3]; ///< uint32_t_array
- uint64_t u64_array[3]; ///< uint64_t_array
- int8_t s8_array[3]; ///< int8_t_array
- int16_t s16_array[3]; ///< int16_t_array
- int32_t s32_array[3]; ///< int32_t_array
- int64_t s64_array[3]; ///< int64_t_array
- float f_array[3]; ///< float_array
- double d_array[3]; ///< double_array
-} mavlink_test_types_t;
-
-#define MAVLINK_MSG_ID_TEST_TYPES_LEN 179
-#define MAVLINK_MSG_ID_0_LEN 179
-
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S_LEN 10
-#define MAVLINK_MSG_TEST_TYPES_FIELD_U8_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_U16_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_U32_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_U64_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S8_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S16_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S32_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S64_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_F_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_D_ARRAY_LEN 3
-
-#define MAVLINK_MESSAGE_INFO_TEST_TYPES { \
-	"TEST_TYPES", \
-	22, \
-	{  { "c", NULL, MAVLINK_TYPE_CHAR, 0, 0, offsetof(mavlink_test_types_t, c) }, \
-         { "s", NULL, MAVLINK_TYPE_CHAR, 10, 1, offsetof(mavlink_test_types_t, s) }, \
-         { "u8", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_test_types_t, u8) }, \
-         { "u16", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_test_types_t, u16) }, \
-         { "u32", "0x%08x", MAVLINK_TYPE_UINT32_T, 0, 14, offsetof(mavlink_test_types_t, u32) }, \
-         { "u64", NULL, MAVLINK_TYPE_UINT64_T, 0, 18, offsetof(mavlink_test_types_t, u64) }, \
-         { "s8", NULL, MAVLINK_TYPE_INT8_T, 0, 26, offsetof(mavlink_test_types_t, s8) }, \
-         { "s16", NULL, MAVLINK_TYPE_INT16_T, 0, 27, offsetof(mavlink_test_types_t, s16) }, \
-         { "s32", NULL, MAVLINK_TYPE_INT32_T, 0, 29, offsetof(mavlink_test_types_t, s32) }, \
-         { "s64", NULL, MAVLINK_TYPE_INT64_T, 0, 33, offsetof(mavlink_test_types_t, s64) }, \
-         { "f", NULL, MAVLINK_TYPE_FLOAT, 0, 41, offsetof(mavlink_test_types_t, f) }, \
-         { "d", NULL, MAVLINK_TYPE_DOUBLE, 0, 45, offsetof(mavlink_test_types_t, d) }, \
-         { "u8_array", NULL, MAVLINK_TYPE_UINT8_T, 3, 53, offsetof(mavlink_test_types_t, u8_array) }, \
-         { "u16_array", NULL, MAVLINK_TYPE_UINT16_T, 3, 56, offsetof(mavlink_test_types_t, u16_array) }, \
-         { "u32_array", NULL, MAVLINK_TYPE_UINT32_T, 3, 62, offsetof(mavlink_test_types_t, u32_array) }, \
-         { "u64_array", NULL, MAVLINK_TYPE_UINT64_T, 3, 74, offsetof(mavlink_test_types_t, u64_array) }, \
-         { "s8_array", NULL, MAVLINK_TYPE_INT8_T, 3, 98, offsetof(mavlink_test_types_t, s8_array) }, \
-         { "s16_array", NULL, MAVLINK_TYPE_INT16_T, 3, 101, offsetof(mavlink_test_types_t, s16_array) }, \
-         { "s32_array", NULL, MAVLINK_TYPE_INT32_T, 3, 107, offsetof(mavlink_test_types_t, s32_array) }, \
-         { "s64_array", NULL, MAVLINK_TYPE_INT64_T, 3, 119, offsetof(mavlink_test_types_t, s64_array) }, \
-         { "f_array", NULL, MAVLINK_TYPE_FLOAT, 3, 143, offsetof(mavlink_test_types_t, f_array) }, \
-         { "d_array", NULL, MAVLINK_TYPE_DOUBLE, 3, 155, offsetof(mavlink_test_types_t, d_array) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a test_types 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 c char
- * @param s string
- * @param u8 uint8_t
- * @param u16 uint16_t
- * @param u32 uint32_t
- * @param u64 uint64_t
- * @param s8 int8_t
- * @param s16 int16_t
- * @param s32 int32_t
- * @param s64 int64_t
- * @param f float
- * @param d double
- * @param u8_array uint8_t_array
- * @param u16_array uint16_t_array
- * @param u32_array uint32_t_array
- * @param u64_array uint64_t_array
- * @param s8_array int8_t_array
- * @param s16_array int16_t_array
- * @param s32_array int32_t_array
- * @param s64_array int64_t_array
- * @param f_array float_array
- * @param d_array double_array
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_test_types_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[179];
-	_mav_put_char(buf, 0, c);
-	_mav_put_uint8_t(buf, 11, u8);
-	_mav_put_uint16_t(buf, 12, u16);
-	_mav_put_uint32_t(buf, 14, u32);
-	_mav_put_uint64_t(buf, 18, u64);
-	_mav_put_int8_t(buf, 26, s8);
-	_mav_put_int16_t(buf, 27, s16);
-	_mav_put_int32_t(buf, 29, s32);
-	_mav_put_int64_t(buf, 33, s64);
-	_mav_put_float(buf, 41, f);
-	_mav_put_double(buf, 45, d);
-	_mav_put_char_array(buf, 1, s, 10);
-	_mav_put_uint8_t_array(buf, 53, u8_array, 3);
-	_mav_put_uint16_t_array(buf, 56, u16_array, 3);
-	_mav_put_uint32_t_array(buf, 62, u32_array, 3);
-	_mav_put_uint64_t_array(buf, 74, u64_array, 3);
-	_mav_put_int8_t_array(buf, 98, s8_array, 3);
-	_mav_put_int16_t_array(buf, 101, s16_array, 3);
-	_mav_put_int32_t_array(buf, 107, s32_array, 3);
-	_mav_put_int64_t_array(buf, 119, s64_array, 3);
-	_mav_put_float_array(buf, 143, f_array, 3);
-	_mav_put_double_array(buf, 155, d_array, 3);
-        memcpy(_MAV_PAYLOAD(msg), buf, 179);
-#else
-	mavlink_test_types_t packet;
-	packet.c = c;
-	packet.u8 = u8;
-	packet.u16 = u16;
-	packet.u32 = u32;
-	packet.u64 = u64;
-	packet.s8 = s8;
-	packet.s16 = s16;
-	packet.s32 = s32;
-	packet.s64 = s64;
-	packet.f = f;
-	packet.d = d;
-	mav_array_memcpy(packet.s, s, sizeof(char)*10);
-	mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
-	mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
-	mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
-	mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
-	mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
-	mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
-	mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
-	mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
-	mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
-	mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
-        memcpy(_MAV_PAYLOAD(msg), &packet, 179);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_TEST_TYPES;
-	return mavlink_finalize_message(msg, system_id, component_id, 179);
-}
-
-/**
- * @brief Pack a test_types 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 was sent over
- * @param msg The MAVLink message to compress the data into
- * @param c char
- * @param s string
- * @param u8 uint8_t
- * @param u16 uint16_t
- * @param u32 uint32_t
- * @param u64 uint64_t
- * @param s8 int8_t
- * @param s16 int16_t
- * @param s32 int32_t
- * @param s64 int64_t
- * @param f float
- * @param d double
- * @param u8_array uint8_t_array
- * @param u16_array uint16_t_array
- * @param u32_array uint32_t_array
- * @param u64_array uint64_t_array
- * @param s8_array int8_t_array
- * @param s16_array int16_t_array
- * @param s32_array int32_t_array
- * @param s64_array int64_t_array
- * @param f_array float_array
- * @param d_array double_array
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[179];
-	_mav_put_char(buf, 0, c);
-	_mav_put_uint8_t(buf, 11, u8);
-	_mav_put_uint16_t(buf, 12, u16);
-	_mav_put_uint32_t(buf, 14, u32);
-	_mav_put_uint64_t(buf, 18, u64);
-	_mav_put_int8_t(buf, 26, s8);
-	_mav_put_int16_t(buf, 27, s16);
-	_mav_put_int32_t(buf, 29, s32);
-	_mav_put_int64_t(buf, 33, s64);
-	_mav_put_float(buf, 41, f);
-	_mav_put_double(buf, 45, d);
-	_mav_put_char_array(buf, 1, s, 10);
-	_mav_put_uint8_t_array(buf, 53, u8_array, 3);
-	_mav_put_uint16_t_array(buf, 56, u16_array, 3);
-	_mav_put_uint32_t_array(buf, 62, u32_array, 3);
-	_mav_put_uint64_t_array(buf, 74, u64_array, 3);
-	_mav_put_int8_t_array(buf, 98, s8_array, 3);
-	_mav_put_int16_t_array(buf, 101, s16_array, 3);
-	_mav_put_int32_t_array(buf, 107, s32_array, 3);
-	_mav_put_int64_t_array(buf, 119, s64_array, 3);
-	_mav_put_float_array(buf, 143, f_array, 3);
-	_mav_put_double_array(buf, 155, d_array, 3);
-        memcpy(_MAV_PAYLOAD(msg), buf, 179);
-#else
-	mavlink_test_types_t packet;
-	packet.c = c;
-	packet.u8 = u8;
-	packet.u16 = u16;
-	packet.u32 = u32;
-	packet.u64 = u64;
-	packet.s8 = s8;
-	packet.s16 = s16;
-	packet.s32 = s32;
-	packet.s64 = s64;
-	packet.f = f;
-	packet.d = d;
-	mav_array_memcpy(packet.s, s, sizeof(char)*10);
-	mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
-	mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
-	mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
-	mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
-	mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
-	mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
-	mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
-	mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
-	mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
-	mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
-        memcpy(_MAV_PAYLOAD(msg), &packet, 179);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_TEST_TYPES;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179);
-}
-
-/**
- * @brief Encode a test_types struct into a 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 test_types C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types)
-{
-	return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array);
-}
-
-/**
- * @brief Send a test_types message
- * @param chan MAVLink channel to send the message
- *
- * @param c char
- * @param s string
- * @param u8 uint8_t
- * @param u16 uint16_t
- * @param u32 uint32_t
- * @param u64 uint64_t
- * @param s8 int8_t
- * @param s16 int16_t
- * @param s32 int32_t
- * @param s64 int64_t
- * @param f float
- * @param d double
- * @param u8_array uint8_t_array
- * @param u16_array uint16_t_array
- * @param u32_array uint32_t_array
- * @param u64_array uint64_t_array
- * @param s8_array int8_t_array
- * @param s16_array int16_t_array
- * @param s32_array int32_t_array
- * @param s64_array int64_t_array
- * @param f_array float_array
- * @param d_array double_array
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[179];
-	_mav_put_char(buf, 0, c);
-	_mav_put_uint8_t(buf, 11, u8);
-	_mav_put_uint16_t(buf, 12, u16);
-	_mav_put_uint32_t(buf, 14, u32);
-	_mav_put_uint64_t(buf, 18, u64);
-	_mav_put_int8_t(buf, 26, s8);
-	_mav_put_int16_t(buf, 27, s16);
-	_mav_put_int32_t(buf, 29, s32);
-	_mav_put_int64_t(buf, 33, s64);
-	_mav_put_float(buf, 41, f);
-	_mav_put_double(buf, 45, d);
-	_mav_put_char_array(buf, 1, s, 10);
-	_mav_put_uint8_t_array(buf, 53, u8_array, 3);
-	_mav_put_uint16_t_array(buf, 56, u16_array, 3);
-	_mav_put_uint32_t_array(buf, 62, u32_array, 3);
-	_mav_put_uint64_t_array(buf, 74, u64_array, 3);
-	_mav_put_int8_t_array(buf, 98, s8_array, 3);
-	_mav_put_int16_t_array(buf, 101, s16_array, 3);
-	_mav_put_int32_t_array(buf, 107, s32_array, 3);
-	_mav_put_int64_t_array(buf, 119, s64_array, 3);
-	_mav_put_float_array(buf, 143, f_array, 3);
-	_mav_put_double_array(buf, 155, d_array, 3);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, buf, 179);
-#else
-	mavlink_test_types_t packet;
-	packet.c = c;
-	packet.u8 = u8;
-	packet.u16 = u16;
-	packet.u32 = u32;
-	packet.u64 = u64;
-	packet.s8 = s8;
-	packet.s16 = s16;
-	packet.s32 = s32;
-	packet.s64 = s64;
-	packet.f = f;
-	packet.d = d;
-	mav_array_memcpy(packet.s, s, sizeof(char)*10);
-	mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
-	mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
-	mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
-	mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
-	mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
-	mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
-	mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
-	mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
-	mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
-	mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, (const char *)&packet, 179);
-#endif
-}
-
-#endif
-
-// MESSAGE TEST_TYPES UNPACKING
-
-
-/**
- * @brief Get field c from test_types message
- *
- * @return char
- */
-static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_char(msg,  0);
-}
-
-/**
- * @brief Get field s from test_types message
- *
- * @return string
- */
-static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s)
-{
-	return _MAV_RETURN_char_array(msg, s, 10,  1);
-}
-
-/**
- * @brief Get field u8 from test_types message
- *
- * @return uint8_t
- */
-static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field u16 from test_types message
- *
- * @return uint16_t
- */
-static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  12);
-}
-
-/**
- * @brief Get field u32 from test_types message
- *
- * @return uint32_t
- */
-static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  14);
-}
-
-/**
- * @brief Get field u64 from test_types message
- *
- * @return uint64_t
- */
-static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  18);
-}
-
-/**
- * @brief Get field s8 from test_types message
- *
- * @return int8_t
- */
-static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int8_t(msg,  26);
-}
-
-/**
- * @brief Get field s16 from test_types message
- *
- * @return int16_t
- */
-static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  27);
-}
-
-/**
- * @brief Get field s32 from test_types message
- *
- * @return int32_t
- */
-static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  29);
-}
-
-/**
- * @brief Get field s64 from test_types message
- *
- * @return int64_t
- */
-static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int64_t(msg,  33);
-}
-
-/**
- * @brief Get field f from test_types message
- *
- * @return float
- */
-static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  41);
-}
-
-/**
- * @brief Get field d from test_types message
- *
- * @return double
- */
-static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_double(msg,  45);
-}
-
-/**
- * @brief Get field u8_array from test_types message
- *
- * @return uint8_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array)
-{
-	return _MAV_RETURN_uint8_t_array(msg, u8_array, 3,  53);
-}
-
-/**
- * @brief Get field u16_array from test_types message
- *
- * @return uint16_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array)
-{
-	return _MAV_RETURN_uint16_t_array(msg, u16_array, 3,  56);
-}
-
-/**
- * @brief Get field u32_array from test_types message
- *
- * @return uint32_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array)
-{
-	return _MAV_RETURN_uint32_t_array(msg, u32_array, 3,  62);
-}
-
-/**
- * @brief Get field u64_array from test_types message
- *
- * @return uint64_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array)
-{
-	return _MAV_RETURN_uint64_t_array(msg, u64_array, 3,  74);
-}
-
-/**
- * @brief Get field s8_array from test_types message
- *
- * @return int8_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array)
-{
-	return _MAV_RETURN_int8_t_array(msg, s8_array, 3,  98);
-}
-
-/**
- * @brief Get field s16_array from test_types message
- *
- * @return int16_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array)
-{
-	return _MAV_RETURN_int16_t_array(msg, s16_array, 3,  101);
-}
-
-/**
- * @brief Get field s32_array from test_types message
- *
- * @return int32_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array)
-{
-	return _MAV_RETURN_int32_t_array(msg, s32_array, 3,  107);
-}
-
-/**
- * @brief Get field s64_array from test_types message
- *
- * @return int64_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array)
-{
-	return _MAV_RETURN_int64_t_array(msg, s64_array, 3,  119);
-}
-
-/**
- * @brief Get field f_array from test_types message
- *
- * @return float_array
- */
-static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array)
-{
-	return _MAV_RETURN_float_array(msg, f_array, 3,  143);
-}
-
-/**
- * @brief Get field d_array from test_types message
- *
- * @return double_array
- */
-static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array)
-{
-	return _MAV_RETURN_double_array(msg, d_array, 3,  155);
-}
-
-/**
- * @brief Decode a test_types message into a struct
- *
- * @param msg The message to decode
- * @param test_types C-struct to decode the message contents into
- */
-static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, mavlink_test_types_t* test_types)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	test_types->c = mavlink_msg_test_types_get_c(msg);
-	mavlink_msg_test_types_get_s(msg, test_types->s);
-	test_types->u8 = mavlink_msg_test_types_get_u8(msg);
-	test_types->u16 = mavlink_msg_test_types_get_u16(msg);
-	test_types->u32 = mavlink_msg_test_types_get_u32(msg);
-	test_types->u64 = mavlink_msg_test_types_get_u64(msg);
-	test_types->s8 = mavlink_msg_test_types_get_s8(msg);
-	test_types->s16 = mavlink_msg_test_types_get_s16(msg);
-	test_types->s32 = mavlink_msg_test_types_get_s32(msg);
-	test_types->s64 = mavlink_msg_test_types_get_s64(msg);
-	test_types->f = mavlink_msg_test_types_get_f(msg);
-	test_types->d = mavlink_msg_test_types_get_d(msg);
-	mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array);
-	mavlink_msg_test_types_get_u16_array(msg, test_types->u16_array);
-	mavlink_msg_test_types_get_u32_array(msg, test_types->u32_array);
-	mavlink_msg_test_types_get_u64_array(msg, test_types->u64_array);
-	mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array);
-	mavlink_msg_test_types_get_s16_array(msg, test_types->s16_array);
-	mavlink_msg_test_types_get_s32_array(msg, test_types->s32_array);
-	mavlink_msg_test_types_get_s64_array(msg, test_types->s64_array);
-	mavlink_msg_test_types_get_f_array(msg, test_types->f_array);
-	mavlink_msg_test_types_get_d_array(msg, test_types->d_array);
-#else
-	memcpy(test_types, _MAV_PAYLOAD(msg), 179);
-#endif
-}
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h
deleted file mode 100644
index 23ee65986..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol generated from test.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef TEST_H
-#define TEST_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// MESSAGE LENGTHS AND CRCS
-
-#ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
-#endif
-
-#ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {91, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
-#endif
-
-#ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_TEST
-
-
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-// ENUM DEFINITIONS
-
-
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_test_types.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // TEST_H
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/testsuite.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/testsuite.h
deleted file mode 100644
index 9b0fc041b..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/testsuite.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from test.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef TEST_TESTSUITE_H
-#define TEST_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-
-static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-
-	mavlink_test_test(system_id, component_id, last_msg);
-}
-#endif
-
-
-
-
-static void mavlink_test_test_types(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_test_types_t packet_in = {
-		'A',
-	"BCDEFGHIJ",
-	230,
-	17859,
-	963498192,
-	93372036854776941ULL,
-	211,
-	18639,
-	963498972,
-	93372036854777886LL,
-	304.0,
-	438.0,
-	{ 228, 229, 230 },
-	{ 20147, 20148, 20149 },
-	{ 963500688, 963500689, 963500690 },
-	{ 93372036854780469, 93372036854780470, 93372036854780471 },
-	{ 171, 172, 173 },
-	{ 22487, 22488, 22489 },
-	{ 963503028, 963503029, 963503030 },
-	{ 93372036854783304, 93372036854783305, 93372036854783306 },
-	{ 1018.0, 1019.0, 1020.0 },
-	{ 1208.0, 1209.0, 1210.0 },
-	};
-	mavlink_test_types_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.c = packet_in.c;
-        	packet1.u8 = packet_in.u8;
-        	packet1.u16 = packet_in.u16;
-        	packet1.u32 = packet_in.u32;
-        	packet1.u64 = packet_in.u64;
-        	packet1.s8 = packet_in.s8;
-        	packet1.s16 = packet_in.s16;
-        	packet1.s32 = packet_in.s32;
-        	packet1.s64 = packet_in.s64;
-        	packet1.f = packet_in.f;
-        	packet1.d = packet_in.d;
-        
-        	mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10);
-        	mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3);
-        	mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3);
-        	mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3);
-        	mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3);
-        	mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3);
-        	mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3);
-        	mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3);
-        	mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3);
-        	mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3);
-        	mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_test_types_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
-	mavlink_msg_test_types_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
-	mavlink_msg_test_types_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_test_types_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_test_types_send(MAVLINK_COMM_1 , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
-	mavlink_msg_test_types_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_test(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_test_types(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // TEST_TESTSUITE_H
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/version.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/version.h
deleted file mode 100644
index 64ca0cb38..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/version.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from test.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "Thu Mar  1 15:11:54 2012"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "0.9"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179
- 
-#endif // MAVLINK_VERSION_H
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/checksum.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/checksum.h
deleted file mode 100644
index 4f4cce02f..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/checksum.h
+++ /dev/null
@@ -1,89 +0,0 @@
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef _CHECKSUM_H_
-#define _CHECKSUM_H_
-
-
-/**
- *
- *  CALCULATE THE CHECKSUM
- *
- */
-
-#define X25_INIT_CRC 0xffff
-#define X25_VALIDATE_CRC 0xf0b8
-
-/**
- * @brief Accumulate the X.25 CRC by adding one char at a time.
- *
- * The checksum function adds the hash of one char at a time to the
- * 16 bit checksum (uint16_t).
- *
- * @param data new char to hash
- * @param crcAccum the already accumulated checksum
- **/
-static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
-{
-        /*Accumulate one byte of data into the CRC*/
-        uint8_t tmp;
-
-        tmp = data ^ (uint8_t)(*crcAccum &0xff);
-        tmp ^= (tmp<<4);
-        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
-}
-
-/**
- * @brief Initiliaze the buffer for the X.25 CRC
- *
- * @param crcAccum the 16 bit X.25 CRC
- */
-static inline void crc_init(uint16_t* crcAccum)
-{
-        *crcAccum = X25_INIT_CRC;
-}
-
-
-/**
- * @brief Calculates the X.25 checksum on a byte buffer
- *
- * @param  pBuffer buffer containing the byte array to hash
- * @param  length  length of the byte array
- * @return the checksum over the buffer bytes
- **/
-static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
-{
-        uint16_t crcTmp;
-        crc_init(&crcTmp);
-	while (length--) {
-                crc_accumulate(*pBuffer++, &crcTmp);
-        }
-        return crcTmp;
-}
-
-/**
- * @brief Accumulate the X.25 CRC by adding an array of bytes
- *
- * The checksum function adds the hash of one char at a time to the
- * 16 bit checksum (uint16_t).
- *
- * @param data new bytes to hash
- * @param crcAccum the already accumulated checksum
- **/
-static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
-{
-	const uint8_t *p = (const uint8_t *)pBuffer;
-	while (length--) {
-                crc_accumulate(*p++, crcAccum);
-        }
-}
-
-
-
-
-#endif /* _CHECKSUM_H_ */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h
deleted file mode 100644
index 39d6930e5..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h
+++ /dev/null
@@ -1,507 +0,0 @@
-#ifndef  _MAVLINK_HELPERS_H_
-#define  _MAVLINK_HELPERS_H_
-
-#include "string.h"
-#include "checksum.h"
-#include "mavlink_types.h"
-
-#ifndef MAVLINK_HELPER
-#define MAVLINK_HELPER
-#endif
-
-/*
-  internal function to give access to the channel status for each channel
- */
-MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
-{
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[chan];
-}
-
-/*
- internal function to give access to the channel buffer for each channel
- */
-MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
-{
-	
-#if MAVLINK_EXTERNAL_RX_BUFFER
-	// No m_mavlink_message array defined in function,
-	// has to be defined externally
-#ifndef m_mavlink_message
-#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION (mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];)
-#endif
-#else
-	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
-#endif
-	return &m_mavlink_buffer[chan];
-}
-
-/**
- * @brief Finalize a MAVLink message with channel assignment
- *
- * This function calculates the checksum and sets length and aircraft id correctly.
- * It assumes that the message id and the payload are already correctly set. This function
- * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
- * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
- *
- * @param msg Message to finalize
- * @param system_id Id of the sending (this) system, 1-127
- * @param length Message length
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length, uint8_t crc_extra)
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length)
-#endif
-{
-	// This code part is the same for all messages;
-	uint16_t checksum;
-	msg->magic = MAVLINK_STX;
-	msg->len = length;
-	msg->sysid = system_id;
-	msg->compid = component_id;
-	// One sequence number per component
-	msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
-	mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
-	checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
-#if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
-#endif
-	mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
-	mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
-
-	return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-}
-
-
-/**
- * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length, uint8_t crc_extra)
-{
-	return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
-}
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length)
-{
-	return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
-}
-#endif
-
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
-
-/**
- * @brief Finalize a MAVLink message with channel assignment and send
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
-						    uint8_t length, uint8_t crc_extra)
-#else
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
-#endif
-{
-	uint16_t checksum;
-	uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
-	uint8_t ck[2];
-	mavlink_status_t *status = mavlink_get_channel_status(chan);
-	buf[0] = MAVLINK_STX;
-	buf[1] = length;
-	buf[2] = status->current_tx_seq;
-	buf[3] = mavlink_system.sysid;
-	buf[4] = mavlink_system.compid;
-	buf[5] = msgid;
-	status->current_tx_seq++;
-	checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
-	crc_accumulate_buffer(&checksum, packet, length);
-#if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
-#endif
-	ck[0] = (uint8_t)(checksum & 0xFF);
-	ck[1] = (uint8_t)(checksum >> 8);
-
-	MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
-	_mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
-	_mavlink_send_uart(chan, packet, length);
-	_mavlink_send_uart(chan, (const char *)ck, 2);
-	MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
-}
-#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-/**
- * @brief Pack a message to send it over a serial byte stream
- */
-MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
-{
-	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
-	return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
-}
-
-union __mavlink_bitfield {
-	uint8_t uint8;
-	int8_t int8;
-	uint16_t uint16;
-	int16_t int16;
-	uint32_t uint32;
-	int32_t int32;
-};
-
-
-MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
-{
-	crc_init(&msg->checksum);
-}
-
-MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
-{
-	crc_accumulate(c, &msg->checksum);
-}
-
-/**
- * This is a convenience function which handles the complete MAVLink parsing.
- * the function will parse one byte at a time and return the complete packet once
- * it could be successfully decoded. Checksum and other failures will be silently
- * ignored.
- *
- * @param chan     ID of the current channel. This allows to parse different channels with this function.
- *                 a channel is not a physical message channel like a serial port, but a logic partition of
- *                 the communication streams in this case. COMM_NB is the limit for the number of channels
- *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
- * @param c        The char to barse
- *
- * @param returnMsg NULL if no message could be decoded, the message data else
- * @return 0 if no message could be decoded, 1 else
- *
- * A typical use scenario of this function call is:
- *
- * @code
- * #include <inttypes.h> // For fixed-width uint8_t type
- *
- * mavlink_message_t msg;
- * int chan = 0;
- *
- *
- * while(serial.bytesAvailable > 0)
- * {
- *   uint8_t byte = serial.getNextByte();
- *   if (mavlink_parse_char(chan, byte, &msg))
- *     {
- *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
- *     }
- * }
- *
- *
- * @endcode
- */
-MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
-{
-        /*
-	  default message crc function. You can override this per-system to
-	  put this data in a different memory segment
-	*/
-#if MAVLINK_CRC_EXTRA
-#ifndef MAVLINK_MESSAGE_CRC
-	static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
-#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
-#endif
-#endif
-
-	mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message
-	mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
-	int bufferIndex = 0;
-
-	status->msg_received = 0;
-
-	switch (status->parse_state)
-	{
-	case MAVLINK_PARSE_STATE_UNINIT:
-	case MAVLINK_PARSE_STATE_IDLE:
-		if (c == MAVLINK_STX)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-			rxmsg->len = 0;
-			rxmsg->magic = c;
-			mavlink_start_checksum(rxmsg);
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_STX:
-			if (status->msg_received 
-/* Support shorter buffers than the
-   default maximum packet size */
-#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
-				|| c > MAVLINK_MAX_PAYLOAD_LEN
-#endif
-				)
-		{
-			status->buffer_overrun++;
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-		}
-		else
-		{
-			// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
-			rxmsg->len = c;
-			status->packet_idx = 0;
-			mavlink_update_checksum(rxmsg, c);
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_LENGTH:
-		rxmsg->seq = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_SEQ:
-		rxmsg->sysid = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_SYSID:
-		rxmsg->compid = c;
-		mavlink_update_checksum(rxmsg, c);
-		status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_COMPID:
-		rxmsg->msgid = c;
-		mavlink_update_checksum(rxmsg, c);
-		if (rxmsg->len == 0)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
-		}
-		else
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_MSGID:
-		_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
-		mavlink_update_checksum(rxmsg, c);
-		if (status->packet_idx == rxmsg->len)
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
-#if MAVLINK_CRC_EXTRA
-		mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
-#endif
-		if (c != (rxmsg->checksum & 0xFF)) {
-			// Check first checksum byte
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			if (c == MAVLINK_STX)
-			{
-				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-				rxmsg->len = 0;
-				mavlink_start_checksum(rxmsg);
-			}
-		}
-		else
-		{
-			status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
-			_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
-		}
-		break;
-
-	case MAVLINK_PARSE_STATE_GOT_CRC1:
-		if (c != (rxmsg->checksum >> 8)) {
-			// Check second checksum byte
-			status->parse_error++;
-			status->msg_received = 0;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			if (c == MAVLINK_STX)
-			{
-				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-				rxmsg->len = 0;
-				mavlink_start_checksum(rxmsg);
-			}
-		}
-		else
-		{
-			// Successfully got message
-			status->msg_received = 1;
-			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-			_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
-			memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
-		}
-		break;
-	}
-
-	bufferIndex++;
-	// If a message has been sucessfully decoded, check index
-	if (status->msg_received == 1)
-	{
-		//while(status->current_seq != rxmsg->seq)
-		//{
-		//	status->packet_rx_drop_count++;
-		//               status->current_seq++;
-		//}
-		status->current_rx_seq = rxmsg->seq;
-		// Initial condition: If no packet has been received so far, drop count is undefined
-		if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
-		// Count this packet as received
-		status->packet_rx_success_count++;
-	}
-
-	r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
-	r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
-	r_mavlink_status->packet_rx_drop_count = status->parse_error;
-	status->parse_error = 0;
-	return status->msg_received;
-}
-
-/**
- * @brief Put a bitfield of length 1-32 bit into the buffer
- *
- * @param b the value to add, will be encoded in the bitfield
- * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
- * @param packet_index the position in the packet (the index of the first byte to use)
- * @param bit_index the position in the byte (the index of the first bit to use)
- * @param buffer packet buffer to write into
- * @return new position of the last used byte in the buffer
- */
-MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
-{
-	uint16_t bits_remain = bits;
-	// Transform number into network order
-	int32_t v;
-	uint8_t i_bit_index, i_byte_index, curr_bits_n;
-#if MAVLINK_NEED_BYTE_SWAP
-	union {
-		int32_t i;
-		uint8_t b[4];
-	} bin, bout;
-	bin.i = b;
-	bout.b[0] = bin.b[3];
-	bout.b[1] = bin.b[2];
-	bout.b[2] = bin.b[1];
-	bout.b[3] = bin.b[0];
-	v = bout.i;
-#else
-	v = b;
-#endif
-
-	// buffer in
-	// 01100000 01000000 00000000 11110001
-	// buffer out
-	// 11110001 00000000 01000000 01100000
-
-	// Existing partly filled byte (four free slots)
-	// 0111xxxx
-
-	// Mask n free bits
-	// 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
-	// = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
-
-	// Shift n bits into the right position
-	// out = in >> n;
-
-	// Mask and shift bytes
-	i_bit_index = bit_index;
-	i_byte_index = packet_index;
-	if (bit_index > 0)
-	{
-		// If bits were available at start, they were available
-		// in the byte before the current index
-		i_byte_index--;
-	}
-
-	// While bits have not been packed yet
-	while (bits_remain > 0)
-	{
-		// Bits still have to be packed
-		// there can be more than 8 bits, so
-		// we might have to pack them into more than one byte
-
-		// First pack everything we can into the current 'open' byte
-		//curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
-		//FIXME
-		if (bits_remain <= (uint8_t)(8 - i_bit_index))
-		{
-			// Enough space
-			curr_bits_n = (uint8_t)bits_remain;
-		}
-		else
-		{
-			curr_bits_n = (8 - i_bit_index);
-		}
-		
-		// Pack these n bits into the current byte
-		// Mask out whatever was at that position with ones (xxx11111)
-		buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
-		// Put content to this position, by masking out the non-used part
-		buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
-		
-		// Increment the bit index
-		i_bit_index += curr_bits_n;
-
-		// Now proceed to the next byte, if necessary
-		bits_remain -= curr_bits_n;
-		if (bits_remain > 0)
-		{
-			// Offer another 8 bits / one byte
-			i_byte_index++;
-			i_bit_index = 0;
-		}
-	}
-	
-	*r_bit_index = i_bit_index;
-	// If a partly filled byte is present, mark this as consumed
-	if (i_bit_index != 7) i_byte_index++;
-	return i_byte_index - packet_index;
-}
-
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-// To make MAVLink work on your MCU, define comm_send_ch() if you wish
-// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
-// whole packet at a time
-
-/*
-
-#include "mavlink_types.h"
-
-void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
-{
-    if (chan == MAVLINK_COMM_0)
-    {
-        uart0_transmit(ch);
-    }
-    if (chan == MAVLINK_COMM_1)
-    {
-    	uart1_transmit(ch);
-    }
-}
- */
-
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
-{
-#ifdef MAVLINK_SEND_UART_BYTES
-	/* this is the more efficient approach, if the platform
-	   defines it */
-	MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len);
-#else
-	/* fallback to one byte at a time */
-	uint16_t i;
-	for (i = 0; i < len; i++) {
-		comm_send_ch(chan, (uint8_t)buf[i]);
-	}
-#endif
-}
-#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-#endif /* _MAVLINK_HELPERS_H_ */
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_protobuf_manager.hpp b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_protobuf_manager.hpp
deleted file mode 100644
index fd3ddd026..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_protobuf_manager.hpp
+++ /dev/null
@@ -1,377 +0,0 @@
-#ifndef MAVLINKPROTOBUFMANAGER_HPP
-#define MAVLINKPROTOBUFMANAGER_HPP
-
-#include <deque>
-#include <google/protobuf/message.h>
-#include <iostream>
-#include <tr1/memory>
-
-#include <checksum.h>
-#include <common/mavlink.h>
-#include <mavlink_types.h>
-#include <pixhawk/pixhawk.pb.h>
-
-namespace mavlink
-{
-
-class ProtobufManager
-{
-public:
-	ProtobufManager()
-	 : mRegisteredTypeCount(0)
-	 , mStreamID(0)
-	 , mVerbose(false)
-	 , kExtendedHeaderSize(MAVLINK_EXTENDED_HEADER_LEN)
-	 , kExtendedPayloadMaxSize(MAVLINK_MAX_EXTENDED_PAYLOAD_LEN)
-	{
-		// register GLOverlay
-		{
-			std::tr1::shared_ptr<px::GLOverlay> msg(new px::GLOverlay);
-			registerType(msg);
-		}
-
-		// register ObstacleList
-		{
-			std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
-			registerType(msg);
-		}
-
-		// register ObstacleMap
-		{
-			std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
-			registerType(msg);
-		}
-
-		// register Path
-		{
-			std::tr1::shared_ptr<px::Path> msg(new px::Path);
-                        registerType(msg);
-		}
-
-		// register PointCloudXYZI
-		{
-			std::tr1::shared_ptr<px::PointCloudXYZI> msg(new px::PointCloudXYZI);
-			registerType(msg);
-		}
-
-		// register PointCloudXYZRGB
-		{
-			std::tr1::shared_ptr<px::PointCloudXYZRGB> msg(new px::PointCloudXYZRGB);
-			registerType(msg);
-		}
-
-		// register RGBDImage
-		{
-			std::tr1::shared_ptr<px::RGBDImage> msg(new px::RGBDImage);
-			registerType(msg);
-		}
-
-		srand(time(NULL));
-		mStreamID = rand() + 1;
-	}
-
-	bool fragmentMessage(uint8_t system_id, uint8_t component_id,
-						 uint8_t target_system, uint8_t target_component,
-						 const google::protobuf::Message& protobuf_msg,
-						 std::vector<mavlink_extended_message_t>& fragments) const
-	{
-		TypeMap::const_iterator it = mTypeMap.find(protobuf_msg.GetTypeName());
-		if (it == mTypeMap.end())
-		{
-			std::cout << "# WARNING: Protobuf message with type "
-					  << protobuf_msg.GetTypeName() << " is not registered."
-					  << std::endl;
-			return false;
-		}
-
-		uint8_t typecode = it->second;
-
-		std::string data = protobuf_msg.SerializeAsString();
-
-		int fragmentCount = (protobuf_msg.ByteSize() + kExtendedPayloadMaxSize - 1) / kExtendedPayloadMaxSize;
-		unsigned int offset = 0;
-
-		for (int i = 0; i < fragmentCount; ++i)
-		{
-			mavlink_extended_message_t fragment;			
-
-			// write extended header data
-			uint8_t* payload = reinterpret_cast<uint8_t*>(fragment.base_msg.payload64);
-			unsigned int length = 0;
-			uint8_t flags = 0;
-
-			if (i < fragmentCount - 1)
-			{
-				length = kExtendedPayloadMaxSize;
-				flags |= 0x1;
-			}
-			else
-			{
-				length = protobuf_msg.ByteSize() - kExtendedPayloadMaxSize * (fragmentCount - 1);
-			}
-
-			memcpy(payload, &target_system, 1);
-			memcpy(payload + 1, &target_component, 1);
-			memcpy(payload + 2, &typecode, 1);
-			memcpy(payload + 3, &length, 4);
-			memcpy(payload + 7, &mStreamID, 2);
-			memcpy(payload + 9, &offset, 4);
-			memcpy(payload + 13, &flags, 1);
-
-			fragment.base_msg.msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE;
-			mavlink_finalize_message(&fragment.base_msg, system_id, component_id, kExtendedHeaderSize, 0);
-			
-			// write extended payload data
-			fragment.extended_payload_len = length;
-			memcpy(fragment.extended_payload, &data[offset], length);
-
-			fragments.push_back(fragment);
-			offset += length;
-		}
-
-		if (mVerbose)
-		{
-			std::cerr << "# INFO: Split extended message with size "
-					  << protobuf_msg.ByteSize() << " into "
-					  << fragmentCount << " fragments." << std::endl;
-		}
-
-		return true;
-	}
-
-	bool cacheFragment(mavlink_extended_message_t& msg)
-	{
-		if (!validFragment(msg))
-		{
-			if (mVerbose)
-			{
-				std::cerr << "# WARNING: Message is not a valid fragment. "
-						  << "Dropping message..." << std::endl;
-			}
-			return false;
-		}
-
-		// read extended header
-		uint8_t* payload = reinterpret_cast<uint8_t*>(msg.base_msg.payload64);
-		uint8_t typecode = 0;
-		unsigned int length = 0;
-		unsigned short streamID = 0;
-		unsigned int offset = 0;
-		uint8_t flags = 0;
-
-		memcpy(&typecode, payload + 2, 1);
-		memcpy(&length, payload + 3, 4);
-		memcpy(&streamID, payload + 7, 2);
-		memcpy(&offset, payload + 9, 4);
-		memcpy(&flags, payload + 13, 1);
-
-		if (typecode >= mTypeMap.size())
-		{
-			std::cout << "# WARNING: Protobuf message with type code "
-					  << static_cast<int>(typecode) << " is not registered." << std::endl;
-			return false;
-		}
-
-		bool reassemble = false;
-
-		FragmentQueue::iterator it = mFragmentQueue.find(streamID);
-		if (it == mFragmentQueue.end())
-		{
-			if (offset == 0)
-			{
-				mFragmentQueue[streamID].push_back(msg);
-
-				if ((flags & 0x1) != 0x1)
-				{
-					reassemble = true;
-				}
-
-				if (mVerbose)
-				{
-					std::cerr << "# INFO: Added fragment to new queue."
-							  << std::endl;
-				}
-			}
-			else
-			{
-				if (mVerbose)
-				{
-					std::cerr << "# WARNING: Message is not a valid fragment. "
-							  << "Dropping message..." << std::endl;
-				}
-			}
-		}
-		else
-		{
-			std::deque<mavlink_extended_message_t>& queue = it->second;
-
-			if (queue.empty())
-			{
-				if (offset == 0)
-				{
-					queue.push_back(msg);
-
-					if ((flags & 0x1) != 0x1)
-					{
-						reassemble = true;
-					}
-				}
-				else
-				{
-					if (mVerbose)
-					{
-						std::cerr << "# WARNING: Message is not a valid fragment. "
-								  << "Dropping message..." << std::endl;
-					}
-				}
-			}
-			else
-			{
-				if (fragmentDataSize(queue.back()) + fragmentOffset(queue.back()) != offset)
-				{
-					if (mVerbose)
-					{
-						std::cerr << "# WARNING: Previous fragment(s) have been lost. "
-								  << "Dropping message and clearing queue..." << std::endl;
-					}
-					queue.clear();
-				}
-				else
-				{
-					queue.push_back(msg);
-
-					if ((flags & 0x1) != 0x1)
-					{
-						reassemble = true;
-					}
-				}
-			}
-		}
-
-		if (reassemble)
-		{
-			std::deque<mavlink_extended_message_t>& queue = mFragmentQueue[streamID];
-
-			std::string data;
-			for (size_t i = 0; i < queue.size(); ++i)
-			{
-				mavlink_extended_message_t& mavlink_msg = queue.at(i);
-
-				data.append(reinterpret_cast<char*>(&mavlink_msg.extended_payload[0]),
-							static_cast<size_t>(mavlink_msg.extended_payload_len));
-			}
-
-			mMessages.at(typecode)->ParseFromString(data);
-
-			mMessageAvailable.at(typecode) = true;
-
-			queue.clear();
-
-			if (mVerbose)
-			{
-				std::cerr << "# INFO: Reassembled fragments for message with typename "
-						  << mMessages.at(typecode)->GetTypeName() << " and size "
-						  << mMessages.at(typecode)->ByteSize()
-						  << "." << std::endl;
-			}
-		}
-
-		return true;
-	}
-
-	bool getMessage(std::tr1::shared_ptr<google::protobuf::Message>& msg)
-	{
-		for (size_t i = 0; i < mMessageAvailable.size(); ++i)
-		{
-			if (mMessageAvailable.at(i))
-			{
-				msg = mMessages.at(i);
-				mMessageAvailable.at(i) = false;
-
-				return true;
-			}
-		}
-
-		return false;
-	}
-
-private:
-	void registerType(const std::tr1::shared_ptr<google::protobuf::Message>& msg)
-	{
-		mTypeMap[msg->GetTypeName()] = mRegisteredTypeCount;
-		++mRegisteredTypeCount;
-		mMessages.push_back(msg);
-		mMessageAvailable.push_back(false);
-	}
-
-	bool validFragment(const mavlink_extended_message_t& msg) const
-	{
-		if (msg.base_msg.magic != MAVLINK_STX ||
-			msg.base_msg.len != kExtendedHeaderSize ||
-			msg.base_msg.msgid != MAVLINK_MSG_ID_EXTENDED_MESSAGE)
-		{
-			return false;
-		}
-
-		uint16_t checksum;
-		checksum = crc_calculate(reinterpret_cast<const uint8_t*>(&msg.base_msg.len), MAVLINK_CORE_HEADER_LEN);
-		crc_accumulate_buffer(&checksum, reinterpret_cast<const char*>(&msg.base_msg.payload64), kExtendedHeaderSize);
-#if MAVLINK_CRC_EXTRA
-		static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
-		crc_accumulate(mavlink_message_crcs[msg.base_msg.msgid], &checksum);
-#endif
-
-		if (mavlink_ck_a(&(msg.base_msg)) != (uint8_t)(checksum & 0xFF) &&
-		    mavlink_ck_b(&(msg.base_msg)) != (uint8_t)(checksum >> 8))
-		{
-			return false;
-		}
-
-		return true;
-	}
-
-	unsigned int fragmentDataSize(const mavlink_extended_message_t& msg) const
-	{
-		const uint8_t* payload = reinterpret_cast<const uint8_t*>(msg.base_msg.payload64);
-
-		return *(reinterpret_cast<const unsigned int*>(payload + 3));
-	}
-
-	unsigned int fragmentOffset(const mavlink_extended_message_t& msg) const
-	{
-		const uint8_t* payload = reinterpret_cast<const uint8_t*>(msg.base_msg.payload64);
-
-		return *(reinterpret_cast<const unsigned int*>(payload + 9));
-	}
-
-	int mRegisteredTypeCount;
-	unsigned short mStreamID;
-	bool mVerbose;
-
-	typedef std::map<std::string, uint8_t> TypeMap;
-	TypeMap mTypeMap;
-	std::vector< std::tr1::shared_ptr<google::protobuf::Message> > mMessages;
-	std::vector<bool> mMessageAvailable;
-
-	typedef std::map<unsigned short, std::deque<mavlink_extended_message_t> > FragmentQueue;
-	FragmentQueue mFragmentQueue;
-
-	const int kExtendedHeaderSize;
-	/**
-	 * Extended header structure
-	 * =========================
-	 *   byte 0 - target_system
-	 *   byte 1 - target_component
-	 *   byte 2 - extended message id (type code)
-	 *   bytes 3-6 - extended payload size in bytes
-	 *   byte 7-8 - stream ID
-	 *   byte 9-12 - fragment offset
-	 *   byte 13 - fragment flags (bit 0 - 1=more fragments, 0=last fragment)
-	 */
-
-	const int kExtendedPayloadMaxSize;
-};
-
-}
-
-#endif
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h
deleted file mode 100644
index 5fbde97f7..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h
+++ /dev/null
@@ -1,158 +0,0 @@
-#ifndef MAVLINK_TYPES_H_
-#define MAVLINK_TYPES_H_
-
-#include <inttypes.h>
-
-#ifndef MAVLINK_MAX_PAYLOAD_LEN
-// it is possible to override this, but be careful!
-#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
-#endif
-
-#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
-#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum
-#define MAVLINK_NUM_CHECKSUM_BYTES 2
-#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
-
-#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
-
-#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
-#define MAVLINK_EXTENDED_HEADER_LEN 14
-
-#if (defined _MSC_VER) | ((defined __APPLE__) & (defined __MACH__)) | (defined __linux__)
-  /* full fledged 32bit++ OS */
-  #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
-#else
-  /* small microcontrollers */
-  #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048
-#endif
-
-#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
-
-typedef struct param_union {
-	union {
-		float param_float;
-		int32_t param_int32;
-		uint32_t param_uint32;
-		uint8_t param_uint8;
-		uint8_t bytes[4];
-	};
-	uint8_t type;
-} mavlink_param_union_t;
-
-typedef struct __mavlink_system {
-    uint8_t sysid;   ///< Used by the MAVLink message_xx_send() convenience function
-    uint8_t compid;  ///< Used by the MAVLink message_xx_send() convenience function
-    uint8_t type;    ///< Unused, can be used by user to store the system's type
-    uint8_t state;   ///< Unused, can be used by user to store the system's state
-    uint8_t mode;    ///< Unused, can be used by user to store the system's mode
-    uint8_t nav_mode;    ///< Unused, can be used by user to store the system's navigation mode
-} mavlink_system_t;
-
-typedef struct __mavlink_message {
-	uint16_t checksum; /// sent at end of packet
-	uint8_t magic;   ///< protocol magic marker
-	uint8_t len;     ///< Length of payload
-	uint8_t seq;     ///< Sequence of packet
-	uint8_t sysid;   ///< ID of message sender system/aircraft
-	uint8_t compid;  ///< ID of the message sender component
-	uint8_t msgid;   ///< ID of message in payload
-	uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
-} mavlink_message_t;
-
-
-typedef struct __mavlink_extended_message {
-       mavlink_message_t base_msg;
-       int32_t extended_payload_len;   ///< Length of extended payload if any
-       uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
-} mavlink_extended_message_t;
-
-
-typedef enum {
-	MAVLINK_TYPE_CHAR     = 0,
-	MAVLINK_TYPE_UINT8_T  = 1,
-	MAVLINK_TYPE_INT8_T   = 2,
-	MAVLINK_TYPE_UINT16_T = 3,
-	MAVLINK_TYPE_INT16_T  = 4,
-	MAVLINK_TYPE_UINT32_T = 5,
-	MAVLINK_TYPE_INT32_T  = 6,
-	MAVLINK_TYPE_UINT64_T = 7,
-	MAVLINK_TYPE_INT64_T  = 8,
-	MAVLINK_TYPE_FLOAT    = 9,
-	MAVLINK_TYPE_DOUBLE   = 10
-} mavlink_message_type_t;
-
-#define MAVLINK_MAX_FIELDS 64
-
-typedef struct __mavlink_field_info {
-        const char *name;                 // name of this field
-        const char *print_format;         // printing format hint, or NULL
-        mavlink_message_type_t type;      // type of this field
-        unsigned int array_length;        // if non-zero, field is an array
-        unsigned int wire_offset;         // offset of each field in the payload
-        unsigned int structure_offset;    // offset in a C structure
-} mavlink_field_info_t;
-
-// note that in this structure the order of fields is the order
-// in the XML file, not necessary the wire order
-typedef struct __mavlink_message_info {
-	const char *name;                                      // name of the message
-	unsigned num_fields;                                   // how many fields in this message
-	mavlink_field_info_t fields[MAVLINK_MAX_FIELDS];       // field information
-} mavlink_message_info_t;
-
-#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0])))
-#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0])))
-
-// checksum is immediately after the payload bytes
-#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
-#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
-
-typedef enum {
-    MAVLINK_COMM_0,
-    MAVLINK_COMM_1,
-    MAVLINK_COMM_2,
-    MAVLINK_COMM_3
-} mavlink_channel_t;
-
-/*
- * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
- * of buffers they will use. If more are used, then the result will be
- * a stack overrun
- */
-#ifndef MAVLINK_COMM_NUM_BUFFERS
-#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
-# define MAVLINK_COMM_NUM_BUFFERS 16
-#else
-# define MAVLINK_COMM_NUM_BUFFERS 4
-#endif
-#endif
-
-typedef enum {
-    MAVLINK_PARSE_STATE_UNINIT=0,
-    MAVLINK_PARSE_STATE_IDLE,
-    MAVLINK_PARSE_STATE_GOT_STX,
-    MAVLINK_PARSE_STATE_GOT_SEQ,
-    MAVLINK_PARSE_STATE_GOT_LENGTH,
-    MAVLINK_PARSE_STATE_GOT_SYSID,
-    MAVLINK_PARSE_STATE_GOT_COMPID,
-    MAVLINK_PARSE_STATE_GOT_MSGID,
-    MAVLINK_PARSE_STATE_GOT_PAYLOAD,
-    MAVLINK_PARSE_STATE_GOT_CRC1
-} mavlink_parse_state_t; ///< The state machine for the comm parser
-
-typedef struct __mavlink_status {
-    uint8_t msg_received;               ///< Number of received messages
-    uint8_t buffer_overrun;             ///< Number of buffer overruns
-    uint8_t parse_error;                ///< Number of parse errors
-    mavlink_parse_state_t parse_state;  ///< Parsing state machine
-    uint8_t packet_idx;                 ///< Index in current packet
-    uint8_t current_rx_seq;             ///< Sequence number of last packet received
-    uint8_t current_tx_seq;             ///< Sequence number of last packet sent
-    uint16_t packet_rx_success_count;   ///< Received packets
-    uint16_t packet_rx_drop_count;      ///< Number of packet drops
-} mavlink_status_t;
-
-#define MAVLINK_BIG_ENDIAN 0
-#define MAVLINK_LITTLE_ENDIAN 1
-
-#endif /* MAVLINK_TYPES_H_ */
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/pixhawk/pixhawk.pb.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/pixhawk/pixhawk.pb.h
deleted file mode 100644
index 7556606e9..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/pixhawk/pixhawk.pb.h
+++ /dev/null
@@ -1,3663 +0,0 @@
-// Generated by the protocol buffer compiler.  DO NOT EDIT!
-// source: pixhawk.proto
-
-#ifndef PROTOBUF_pixhawk_2eproto__INCLUDED
-#define PROTOBUF_pixhawk_2eproto__INCLUDED
-
-#include <string>
-
-#include <google/protobuf/stubs/common.h>
-
-#if GOOGLE_PROTOBUF_VERSION < 2004000
-#error This file was generated by a newer version of protoc which is
-#error incompatible with your Protocol Buffer headers.  Please update
-#error your headers.
-#endif
-#if 2004001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
-#error This file was generated by an older version of protoc which is
-#error incompatible with your Protocol Buffer headers.  Please
-#error regenerate this file with a newer version of protoc.
-#endif
-
-#include <google/protobuf/generated_message_util.h>
-#include <google/protobuf/repeated_field.h>
-#include <google/protobuf/extension_set.h>
-#include <google/protobuf/generated_message_reflection.h>
-// @@protoc_insertion_point(includes)
-
-namespace px {
-
-// Internal implementation detail -- do not call these.
-void  protobuf_AddDesc_pixhawk_2eproto();
-void protobuf_AssignDesc_pixhawk_2eproto();
-void protobuf_ShutdownFile_pixhawk_2eproto();
-
-class HeaderInfo;
-class GLOverlay;
-class Obstacle;
-class ObstacleList;
-class ObstacleMap;
-class Path;
-class PointCloudXYZI;
-class PointCloudXYZI_PointXYZI;
-class PointCloudXYZRGB;
-class PointCloudXYZRGB_PointXYZRGB;
-class RGBDImage;
-class Waypoint;
-
-enum GLOverlay_CoordinateFrameType {
-  GLOverlay_CoordinateFrameType_GLOBAL = 0,
-  GLOverlay_CoordinateFrameType_LOCAL = 1
-};
-bool GLOverlay_CoordinateFrameType_IsValid(int value);
-const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN = GLOverlay_CoordinateFrameType_GLOBAL;
-const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX = GLOverlay_CoordinateFrameType_LOCAL;
-const int GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE = GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX + 1;
-
-const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor();
-inline const ::std::string& GLOverlay_CoordinateFrameType_Name(GLOverlay_CoordinateFrameType value) {
-  return ::google::protobuf::internal::NameOfEnum(
-    GLOverlay_CoordinateFrameType_descriptor(), value);
-}
-inline bool GLOverlay_CoordinateFrameType_Parse(
-    const ::std::string& name, GLOverlay_CoordinateFrameType* value) {
-  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_CoordinateFrameType>(
-    GLOverlay_CoordinateFrameType_descriptor(), name, value);
-}
-enum GLOverlay_Mode {
-  GLOverlay_Mode_POINTS = 0,
-  GLOverlay_Mode_LINES = 1,
-  GLOverlay_Mode_LINE_STRIP = 2,
-  GLOverlay_Mode_LINE_LOOP = 3,
-  GLOverlay_Mode_TRIANGLES = 4,
-  GLOverlay_Mode_TRIANGLE_STRIP = 5,
-  GLOverlay_Mode_TRIANGLE_FAN = 6,
-  GLOverlay_Mode_QUADS = 7,
-  GLOverlay_Mode_QUAD_STRIP = 8,
-  GLOverlay_Mode_POLYGON = 9,
-  GLOverlay_Mode_SOLID_CIRCLE = 10,
-  GLOverlay_Mode_WIRE_CIRCLE = 11,
-  GLOverlay_Mode_SOLID_CUBE = 12,
-  GLOverlay_Mode_WIRE_CUBE = 13
-};
-bool GLOverlay_Mode_IsValid(int value);
-const GLOverlay_Mode GLOverlay_Mode_Mode_MIN = GLOverlay_Mode_POINTS;
-const GLOverlay_Mode GLOverlay_Mode_Mode_MAX = GLOverlay_Mode_WIRE_CUBE;
-const int GLOverlay_Mode_Mode_ARRAYSIZE = GLOverlay_Mode_Mode_MAX + 1;
-
-const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor();
-inline const ::std::string& GLOverlay_Mode_Name(GLOverlay_Mode value) {
-  return ::google::protobuf::internal::NameOfEnum(
-    GLOverlay_Mode_descriptor(), value);
-}
-inline bool GLOverlay_Mode_Parse(
-    const ::std::string& name, GLOverlay_Mode* value) {
-  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_Mode>(
-    GLOverlay_Mode_descriptor(), name, value);
-}
-enum GLOverlay_Identifier {
-  GLOverlay_Identifier_END = 14,
-  GLOverlay_Identifier_VERTEX2F = 15,
-  GLOverlay_Identifier_VERTEX3F = 16,
-  GLOverlay_Identifier_ROTATEF = 17,
-  GLOverlay_Identifier_TRANSLATEF = 18,
-  GLOverlay_Identifier_SCALEF = 19,
-  GLOverlay_Identifier_PUSH_MATRIX = 20,
-  GLOverlay_Identifier_POP_MATRIX = 21,
-  GLOverlay_Identifier_COLOR3F = 22,
-  GLOverlay_Identifier_COLOR4F = 23,
-  GLOverlay_Identifier_POINTSIZE = 24,
-  GLOverlay_Identifier_LINEWIDTH = 25
-};
-bool GLOverlay_Identifier_IsValid(int value);
-const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MIN = GLOverlay_Identifier_END;
-const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MAX = GLOverlay_Identifier_LINEWIDTH;
-const int GLOverlay_Identifier_Identifier_ARRAYSIZE = GLOverlay_Identifier_Identifier_MAX + 1;
-
-const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor();
-inline const ::std::string& GLOverlay_Identifier_Name(GLOverlay_Identifier value) {
-  return ::google::protobuf::internal::NameOfEnum(
-    GLOverlay_Identifier_descriptor(), value);
-}
-inline bool GLOverlay_Identifier_Parse(
-    const ::std::string& name, GLOverlay_Identifier* value) {
-  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_Identifier>(
-    GLOverlay_Identifier_descriptor(), name, value);
-}
-// ===================================================================
-
-class HeaderInfo : public ::google::protobuf::Message {
- public:
-  HeaderInfo();
-  virtual ~HeaderInfo();
-  
-  HeaderInfo(const HeaderInfo& from);
-  
-  inline HeaderInfo& operator=(const HeaderInfo& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const HeaderInfo& default_instance();
-  
-  void Swap(HeaderInfo* other);
-  
-  // implements Message ----------------------------------------------
-  
-  HeaderInfo* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const HeaderInfo& from);
-  void MergeFrom(const HeaderInfo& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required int32 source_sysid = 1;
-  inline bool has_source_sysid() const;
-  inline void clear_source_sysid();
-  static const int kSourceSysidFieldNumber = 1;
-  inline ::google::protobuf::int32 source_sysid() const;
-  inline void set_source_sysid(::google::protobuf::int32 value);
-  
-  // required int32 source_compid = 2;
-  inline bool has_source_compid() const;
-  inline void clear_source_compid();
-  static const int kSourceCompidFieldNumber = 2;
-  inline ::google::protobuf::int32 source_compid() const;
-  inline void set_source_compid(::google::protobuf::int32 value);
-  
-  // required double timestamp = 3;
-  inline bool has_timestamp() const;
-  inline void clear_timestamp();
-  static const int kTimestampFieldNumber = 3;
-  inline double timestamp() const;
-  inline void set_timestamp(double value);
-  
-  // @@protoc_insertion_point(class_scope:px.HeaderInfo)
- private:
-  inline void set_has_source_sysid();
-  inline void clear_has_source_sysid();
-  inline void set_has_source_compid();
-  inline void clear_has_source_compid();
-  inline void set_has_timestamp();
-  inline void clear_has_timestamp();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::google::protobuf::int32 source_sysid_;
-  ::google::protobuf::int32 source_compid_;
-  double timestamp_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(3 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static HeaderInfo* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class GLOverlay : public ::google::protobuf::Message {
- public:
-  GLOverlay();
-  virtual ~GLOverlay();
-  
-  GLOverlay(const GLOverlay& from);
-  
-  inline GLOverlay& operator=(const GLOverlay& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const GLOverlay& default_instance();
-  
-  void Swap(GLOverlay* other);
-  
-  // implements Message ----------------------------------------------
-  
-  GLOverlay* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const GLOverlay& from);
-  void MergeFrom(const GLOverlay& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  typedef GLOverlay_CoordinateFrameType CoordinateFrameType;
-  static const CoordinateFrameType GLOBAL = GLOverlay_CoordinateFrameType_GLOBAL;
-  static const CoordinateFrameType LOCAL = GLOverlay_CoordinateFrameType_LOCAL;
-  static inline bool CoordinateFrameType_IsValid(int value) {
-    return GLOverlay_CoordinateFrameType_IsValid(value);
-  }
-  static const CoordinateFrameType CoordinateFrameType_MIN =
-    GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN;
-  static const CoordinateFrameType CoordinateFrameType_MAX =
-    GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX;
-  static const int CoordinateFrameType_ARRAYSIZE =
-    GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE;
-  static inline const ::google::protobuf::EnumDescriptor*
-  CoordinateFrameType_descriptor() {
-    return GLOverlay_CoordinateFrameType_descriptor();
-  }
-  static inline const ::std::string& CoordinateFrameType_Name(CoordinateFrameType value) {
-    return GLOverlay_CoordinateFrameType_Name(value);
-  }
-  static inline bool CoordinateFrameType_Parse(const ::std::string& name,
-      CoordinateFrameType* value) {
-    return GLOverlay_CoordinateFrameType_Parse(name, value);
-  }
-  
-  typedef GLOverlay_Mode Mode;
-  static const Mode POINTS = GLOverlay_Mode_POINTS;
-  static const Mode LINES = GLOverlay_Mode_LINES;
-  static const Mode LINE_STRIP = GLOverlay_Mode_LINE_STRIP;
-  static const Mode LINE_LOOP = GLOverlay_Mode_LINE_LOOP;
-  static const Mode TRIANGLES = GLOverlay_Mode_TRIANGLES;
-  static const Mode TRIANGLE_STRIP = GLOverlay_Mode_TRIANGLE_STRIP;
-  static const Mode TRIANGLE_FAN = GLOverlay_Mode_TRIANGLE_FAN;
-  static const Mode QUADS = GLOverlay_Mode_QUADS;
-  static const Mode QUAD_STRIP = GLOverlay_Mode_QUAD_STRIP;
-  static const Mode POLYGON = GLOverlay_Mode_POLYGON;
-  static const Mode SOLID_CIRCLE = GLOverlay_Mode_SOLID_CIRCLE;
-  static const Mode WIRE_CIRCLE = GLOverlay_Mode_WIRE_CIRCLE;
-  static const Mode SOLID_CUBE = GLOverlay_Mode_SOLID_CUBE;
-  static const Mode WIRE_CUBE = GLOverlay_Mode_WIRE_CUBE;
-  static inline bool Mode_IsValid(int value) {
-    return GLOverlay_Mode_IsValid(value);
-  }
-  static const Mode Mode_MIN =
-    GLOverlay_Mode_Mode_MIN;
-  static const Mode Mode_MAX =
-    GLOverlay_Mode_Mode_MAX;
-  static const int Mode_ARRAYSIZE =
-    GLOverlay_Mode_Mode_ARRAYSIZE;
-  static inline const ::google::protobuf::EnumDescriptor*
-  Mode_descriptor() {
-    return GLOverlay_Mode_descriptor();
-  }
-  static inline const ::std::string& Mode_Name(Mode value) {
-    return GLOverlay_Mode_Name(value);
-  }
-  static inline bool Mode_Parse(const ::std::string& name,
-      Mode* value) {
-    return GLOverlay_Mode_Parse(name, value);
-  }
-  
-  typedef GLOverlay_Identifier Identifier;
-  static const Identifier END = GLOverlay_Identifier_END;
-  static const Identifier VERTEX2F = GLOverlay_Identifier_VERTEX2F;
-  static const Identifier VERTEX3F = GLOverlay_Identifier_VERTEX3F;
-  static const Identifier ROTATEF = GLOverlay_Identifier_ROTATEF;
-  static const Identifier TRANSLATEF = GLOverlay_Identifier_TRANSLATEF;
-  static const Identifier SCALEF = GLOverlay_Identifier_SCALEF;
-  static const Identifier PUSH_MATRIX = GLOverlay_Identifier_PUSH_MATRIX;
-  static const Identifier POP_MATRIX = GLOverlay_Identifier_POP_MATRIX;
-  static const Identifier COLOR3F = GLOverlay_Identifier_COLOR3F;
-  static const Identifier COLOR4F = GLOverlay_Identifier_COLOR4F;
-  static const Identifier POINTSIZE = GLOverlay_Identifier_POINTSIZE;
-  static const Identifier LINEWIDTH = GLOverlay_Identifier_LINEWIDTH;
-  static inline bool Identifier_IsValid(int value) {
-    return GLOverlay_Identifier_IsValid(value);
-  }
-  static const Identifier Identifier_MIN =
-    GLOverlay_Identifier_Identifier_MIN;
-  static const Identifier Identifier_MAX =
-    GLOverlay_Identifier_Identifier_MAX;
-  static const int Identifier_ARRAYSIZE =
-    GLOverlay_Identifier_Identifier_ARRAYSIZE;
-  static inline const ::google::protobuf::EnumDescriptor*
-  Identifier_descriptor() {
-    return GLOverlay_Identifier_descriptor();
-  }
-  static inline const ::std::string& Identifier_Name(Identifier value) {
-    return GLOverlay_Identifier_Name(value);
-  }
-  static inline bool Identifier_Parse(const ::std::string& name,
-      Identifier* value) {
-    return GLOverlay_Identifier_Parse(name, value);
-  }
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // optional string name = 2;
-  inline bool has_name() const;
-  inline void clear_name();
-  static const int kNameFieldNumber = 2;
-  inline const ::std::string& name() const;
-  inline void set_name(const ::std::string& value);
-  inline void set_name(const char* value);
-  inline void set_name(const char* value, size_t size);
-  inline ::std::string* mutable_name();
-  inline ::std::string* release_name();
-  
-  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-  inline bool has_coordinateframetype() const;
-  inline void clear_coordinateframetype();
-  static const int kCoordinateFrameTypeFieldNumber = 3;
-  inline ::px::GLOverlay_CoordinateFrameType coordinateframetype() const;
-  inline void set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value);
-  
-  // optional double origin_x = 4;
-  inline bool has_origin_x() const;
-  inline void clear_origin_x();
-  static const int kOriginXFieldNumber = 4;
-  inline double origin_x() const;
-  inline void set_origin_x(double value);
-  
-  // optional double origin_y = 5;
-  inline bool has_origin_y() const;
-  inline void clear_origin_y();
-  static const int kOriginYFieldNumber = 5;
-  inline double origin_y() const;
-  inline void set_origin_y(double value);
-  
-  // optional double origin_z = 6;
-  inline bool has_origin_z() const;
-  inline void clear_origin_z();
-  static const int kOriginZFieldNumber = 6;
-  inline double origin_z() const;
-  inline void set_origin_z(double value);
-  
-  // optional bytes data = 7;
-  inline bool has_data() const;
-  inline void clear_data();
-  static const int kDataFieldNumber = 7;
-  inline const ::std::string& data() const;
-  inline void set_data(const ::std::string& value);
-  inline void set_data(const char* value);
-  inline void set_data(const void* value, size_t size);
-  inline ::std::string* mutable_data();
-  inline ::std::string* release_data();
-  
-  // @@protoc_insertion_point(class_scope:px.GLOverlay)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  inline void set_has_name();
-  inline void clear_has_name();
-  inline void set_has_coordinateframetype();
-  inline void clear_has_coordinateframetype();
-  inline void set_has_origin_x();
-  inline void clear_has_origin_x();
-  inline void set_has_origin_y();
-  inline void clear_has_origin_y();
-  inline void set_has_origin_z();
-  inline void clear_has_origin_z();
-  inline void set_has_data();
-  inline void clear_has_data();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::std::string* name_;
-  double origin_x_;
-  double origin_y_;
-  double origin_z_;
-  ::std::string* data_;
-  int coordinateframetype_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(7 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static GLOverlay* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class Obstacle : public ::google::protobuf::Message {
- public:
-  Obstacle();
-  virtual ~Obstacle();
-  
-  Obstacle(const Obstacle& from);
-  
-  inline Obstacle& operator=(const Obstacle& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const Obstacle& default_instance();
-  
-  void Swap(Obstacle* other);
-  
-  // implements Message ----------------------------------------------
-  
-  Obstacle* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const Obstacle& from);
-  void MergeFrom(const Obstacle& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // optional float x = 1;
-  inline bool has_x() const;
-  inline void clear_x();
-  static const int kXFieldNumber = 1;
-  inline float x() const;
-  inline void set_x(float value);
-  
-  // optional float y = 2;
-  inline bool has_y() const;
-  inline void clear_y();
-  static const int kYFieldNumber = 2;
-  inline float y() const;
-  inline void set_y(float value);
-  
-  // optional float z = 3;
-  inline bool has_z() const;
-  inline void clear_z();
-  static const int kZFieldNumber = 3;
-  inline float z() const;
-  inline void set_z(float value);
-  
-  // optional float length = 4;
-  inline bool has_length() const;
-  inline void clear_length();
-  static const int kLengthFieldNumber = 4;
-  inline float length() const;
-  inline void set_length(float value);
-  
-  // optional float width = 5;
-  inline bool has_width() const;
-  inline void clear_width();
-  static const int kWidthFieldNumber = 5;
-  inline float width() const;
-  inline void set_width(float value);
-  
-  // optional float height = 6;
-  inline bool has_height() const;
-  inline void clear_height();
-  static const int kHeightFieldNumber = 6;
-  inline float height() const;
-  inline void set_height(float value);
-  
-  // @@protoc_insertion_point(class_scope:px.Obstacle)
- private:
-  inline void set_has_x();
-  inline void clear_has_x();
-  inline void set_has_y();
-  inline void clear_has_y();
-  inline void set_has_z();
-  inline void clear_has_z();
-  inline void set_has_length();
-  inline void clear_has_length();
-  inline void set_has_width();
-  inline void clear_has_width();
-  inline void set_has_height();
-  inline void clear_has_height();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  float x_;
-  float y_;
-  float z_;
-  float length_;
-  float width_;
-  float height_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static Obstacle* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class ObstacleList : public ::google::protobuf::Message {
- public:
-  ObstacleList();
-  virtual ~ObstacleList();
-  
-  ObstacleList(const ObstacleList& from);
-  
-  inline ObstacleList& operator=(const ObstacleList& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const ObstacleList& default_instance();
-  
-  void Swap(ObstacleList* other);
-  
-  // implements Message ----------------------------------------------
-  
-  ObstacleList* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const ObstacleList& from);
-  void MergeFrom(const ObstacleList& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.Obstacle obstacles = 2;
-  inline int obstacles_size() const;
-  inline void clear_obstacles();
-  static const int kObstaclesFieldNumber = 2;
-  inline const ::px::Obstacle& obstacles(int index) const;
-  inline ::px::Obstacle* mutable_obstacles(int index);
-  inline ::px::Obstacle* add_obstacles();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
-      obstacles() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
-      mutable_obstacles();
-  
-  // @@protoc_insertion_point(class_scope:px.ObstacleList)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static ObstacleList* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class ObstacleMap : public ::google::protobuf::Message {
- public:
-  ObstacleMap();
-  virtual ~ObstacleMap();
-  
-  ObstacleMap(const ObstacleMap& from);
-  
-  inline ObstacleMap& operator=(const ObstacleMap& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const ObstacleMap& default_instance();
-  
-  void Swap(ObstacleMap* other);
-  
-  // implements Message ----------------------------------------------
-  
-  ObstacleMap* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const ObstacleMap& from);
-  void MergeFrom(const ObstacleMap& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // required int32 type = 2;
-  inline bool has_type() const;
-  inline void clear_type();
-  static const int kTypeFieldNumber = 2;
-  inline ::google::protobuf::int32 type() const;
-  inline void set_type(::google::protobuf::int32 value);
-  
-  // optional float resolution = 3;
-  inline bool has_resolution() const;
-  inline void clear_resolution();
-  static const int kResolutionFieldNumber = 3;
-  inline float resolution() const;
-  inline void set_resolution(float value);
-  
-  // optional int32 rows = 4;
-  inline bool has_rows() const;
-  inline void clear_rows();
-  static const int kRowsFieldNumber = 4;
-  inline ::google::protobuf::int32 rows() const;
-  inline void set_rows(::google::protobuf::int32 value);
-  
-  // optional int32 cols = 5;
-  inline bool has_cols() const;
-  inline void clear_cols();
-  static const int kColsFieldNumber = 5;
-  inline ::google::protobuf::int32 cols() const;
-  inline void set_cols(::google::protobuf::int32 value);
-  
-  // optional int32 mapR0 = 6;
-  inline bool has_mapr0() const;
-  inline void clear_mapr0();
-  static const int kMapR0FieldNumber = 6;
-  inline ::google::protobuf::int32 mapr0() const;
-  inline void set_mapr0(::google::protobuf::int32 value);
-  
-  // optional int32 mapC0 = 7;
-  inline bool has_mapc0() const;
-  inline void clear_mapc0();
-  static const int kMapC0FieldNumber = 7;
-  inline ::google::protobuf::int32 mapc0() const;
-  inline void set_mapc0(::google::protobuf::int32 value);
-  
-  // optional int32 arrayR0 = 8;
-  inline bool has_arrayr0() const;
-  inline void clear_arrayr0();
-  static const int kArrayR0FieldNumber = 8;
-  inline ::google::protobuf::int32 arrayr0() const;
-  inline void set_arrayr0(::google::protobuf::int32 value);
-  
-  // optional int32 arrayC0 = 9;
-  inline bool has_arrayc0() const;
-  inline void clear_arrayc0();
-  static const int kArrayC0FieldNumber = 9;
-  inline ::google::protobuf::int32 arrayc0() const;
-  inline void set_arrayc0(::google::protobuf::int32 value);
-  
-  // optional bytes data = 10;
-  inline bool has_data() const;
-  inline void clear_data();
-  static const int kDataFieldNumber = 10;
-  inline const ::std::string& data() const;
-  inline void set_data(const ::std::string& value);
-  inline void set_data(const char* value);
-  inline void set_data(const void* value, size_t size);
-  inline ::std::string* mutable_data();
-  inline ::std::string* release_data();
-  
-  // @@protoc_insertion_point(class_scope:px.ObstacleMap)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  inline void set_has_type();
-  inline void clear_has_type();
-  inline void set_has_resolution();
-  inline void clear_has_resolution();
-  inline void set_has_rows();
-  inline void clear_has_rows();
-  inline void set_has_cols();
-  inline void clear_has_cols();
-  inline void set_has_mapr0();
-  inline void clear_has_mapr0();
-  inline void set_has_mapc0();
-  inline void clear_has_mapc0();
-  inline void set_has_arrayr0();
-  inline void clear_has_arrayr0();
-  inline void set_has_arrayc0();
-  inline void clear_has_arrayc0();
-  inline void set_has_data();
-  inline void clear_has_data();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::int32 type_;
-  float resolution_;
-  ::google::protobuf::int32 rows_;
-  ::google::protobuf::int32 cols_;
-  ::google::protobuf::int32 mapr0_;
-  ::google::protobuf::int32 mapc0_;
-  ::google::protobuf::int32 arrayr0_;
-  ::google::protobuf::int32 arrayc0_;
-  ::std::string* data_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(10 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static ObstacleMap* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class Path : public ::google::protobuf::Message {
- public:
-  Path();
-  virtual ~Path();
-  
-  Path(const Path& from);
-  
-  inline Path& operator=(const Path& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const Path& default_instance();
-  
-  void Swap(Path* other);
-  
-  // implements Message ----------------------------------------------
-  
-  Path* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const Path& from);
-  void MergeFrom(const Path& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.Waypoint waypoints = 2;
-  inline int waypoints_size() const;
-  inline void clear_waypoints();
-  static const int kWaypointsFieldNumber = 2;
-  inline const ::px::Waypoint& waypoints(int index) const;
-  inline ::px::Waypoint* mutable_waypoints(int index);
-  inline ::px::Waypoint* add_waypoints();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >&
-      waypoints() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >*
-      mutable_waypoints();
-  
-  // @@protoc_insertion_point(class_scope:px.Path)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::Waypoint > waypoints_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static Path* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message {
- public:
-  PointCloudXYZI_PointXYZI();
-  virtual ~PointCloudXYZI_PointXYZI();
-  
-  PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from);
-  
-  inline PointCloudXYZI_PointXYZI& operator=(const PointCloudXYZI_PointXYZI& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZI_PointXYZI& default_instance();
-  
-  void Swap(PointCloudXYZI_PointXYZI* other);
-  
-  // implements Message ----------------------------------------------
-  
-  PointCloudXYZI_PointXYZI* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZI_PointXYZI& from);
-  void MergeFrom(const PointCloudXYZI_PointXYZI& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required float x = 1;
-  inline bool has_x() const;
-  inline void clear_x();
-  static const int kXFieldNumber = 1;
-  inline float x() const;
-  inline void set_x(float value);
-  
-  // required float y = 2;
-  inline bool has_y() const;
-  inline void clear_y();
-  static const int kYFieldNumber = 2;
-  inline float y() const;
-  inline void set_y(float value);
-  
-  // required float z = 3;
-  inline bool has_z() const;
-  inline void clear_z();
-  static const int kZFieldNumber = 3;
-  inline float z() const;
-  inline void set_z(float value);
-  
-  // required float intensity = 4;
-  inline bool has_intensity() const;
-  inline void clear_intensity();
-  static const int kIntensityFieldNumber = 4;
-  inline float intensity() const;
-  inline void set_intensity(float value);
-  
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZI.PointXYZI)
- private:
-  inline void set_has_x();
-  inline void clear_has_x();
-  inline void set_has_y();
-  inline void clear_has_y();
-  inline void set_has_z();
-  inline void clear_has_z();
-  inline void set_has_intensity();
-  inline void clear_has_intensity();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  float x_;
-  float y_;
-  float z_;
-  float intensity_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static PointCloudXYZI_PointXYZI* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class PointCloudXYZI : public ::google::protobuf::Message {
- public:
-  PointCloudXYZI();
-  virtual ~PointCloudXYZI();
-  
-  PointCloudXYZI(const PointCloudXYZI& from);
-  
-  inline PointCloudXYZI& operator=(const PointCloudXYZI& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZI& default_instance();
-  
-  void Swap(PointCloudXYZI* other);
-  
-  // implements Message ----------------------------------------------
-  
-  PointCloudXYZI* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZI& from);
-  void MergeFrom(const PointCloudXYZI& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  typedef PointCloudXYZI_PointXYZI PointXYZI;
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  inline int points_size() const;
-  inline void clear_points();
-  static const int kPointsFieldNumber = 2;
-  inline const ::px::PointCloudXYZI_PointXYZI& points(int index) const;
-  inline ::px::PointCloudXYZI_PointXYZI* mutable_points(int index);
-  inline ::px::PointCloudXYZI_PointXYZI* add_points();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >&
-      points() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >*
-      mutable_points();
-  
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZI)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI > points_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static PointCloudXYZI* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message {
- public:
-  PointCloudXYZRGB_PointXYZRGB();
-  virtual ~PointCloudXYZRGB_PointXYZRGB();
-  
-  PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from);
-  
-  inline PointCloudXYZRGB_PointXYZRGB& operator=(const PointCloudXYZRGB_PointXYZRGB& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZRGB_PointXYZRGB& default_instance();
-  
-  void Swap(PointCloudXYZRGB_PointXYZRGB* other);
-  
-  // implements Message ----------------------------------------------
-  
-  PointCloudXYZRGB_PointXYZRGB* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from);
-  void MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required float x = 1;
-  inline bool has_x() const;
-  inline void clear_x();
-  static const int kXFieldNumber = 1;
-  inline float x() const;
-  inline void set_x(float value);
-  
-  // required float y = 2;
-  inline bool has_y() const;
-  inline void clear_y();
-  static const int kYFieldNumber = 2;
-  inline float y() const;
-  inline void set_y(float value);
-  
-  // required float z = 3;
-  inline bool has_z() const;
-  inline void clear_z();
-  static const int kZFieldNumber = 3;
-  inline float z() const;
-  inline void set_z(float value);
-  
-  // required float rgb = 4;
-  inline bool has_rgb() const;
-  inline void clear_rgb();
-  static const int kRgbFieldNumber = 4;
-  inline float rgb() const;
-  inline void set_rgb(float value);
-  
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB.PointXYZRGB)
- private:
-  inline void set_has_x();
-  inline void clear_has_x();
-  inline void set_has_y();
-  inline void clear_has_y();
-  inline void set_has_z();
-  inline void clear_has_z();
-  inline void set_has_rgb();
-  inline void clear_has_rgb();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  float x_;
-  float y_;
-  float z_;
-  float rgb_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static PointCloudXYZRGB_PointXYZRGB* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class PointCloudXYZRGB : public ::google::protobuf::Message {
- public:
-  PointCloudXYZRGB();
-  virtual ~PointCloudXYZRGB();
-  
-  PointCloudXYZRGB(const PointCloudXYZRGB& from);
-  
-  inline PointCloudXYZRGB& operator=(const PointCloudXYZRGB& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZRGB& default_instance();
-  
-  void Swap(PointCloudXYZRGB* other);
-  
-  // implements Message ----------------------------------------------
-  
-  PointCloudXYZRGB* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZRGB& from);
-  void MergeFrom(const PointCloudXYZRGB& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  typedef PointCloudXYZRGB_PointXYZRGB PointXYZRGB;
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  inline int points_size() const;
-  inline void clear_points();
-  static const int kPointsFieldNumber = 2;
-  inline const ::px::PointCloudXYZRGB_PointXYZRGB& points(int index) const;
-  inline ::px::PointCloudXYZRGB_PointXYZRGB* mutable_points(int index);
-  inline ::px::PointCloudXYZRGB_PointXYZRGB* add_points();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >&
-      points() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >*
-      mutable_points();
-  
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB > points_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static PointCloudXYZRGB* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class RGBDImage : public ::google::protobuf::Message {
- public:
-  RGBDImage();
-  virtual ~RGBDImage();
-  
-  RGBDImage(const RGBDImage& from);
-  
-  inline RGBDImage& operator=(const RGBDImage& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const RGBDImage& default_instance();
-  
-  void Swap(RGBDImage* other);
-  
-  // implements Message ----------------------------------------------
-  
-  RGBDImage* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const RGBDImage& from);
-  void MergeFrom(const RGBDImage& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // required uint32 cols = 2;
-  inline bool has_cols() const;
-  inline void clear_cols();
-  static const int kColsFieldNumber = 2;
-  inline ::google::protobuf::uint32 cols() const;
-  inline void set_cols(::google::protobuf::uint32 value);
-  
-  // required uint32 rows = 3;
-  inline bool has_rows() const;
-  inline void clear_rows();
-  static const int kRowsFieldNumber = 3;
-  inline ::google::protobuf::uint32 rows() const;
-  inline void set_rows(::google::protobuf::uint32 value);
-  
-  // required uint32 step1 = 4;
-  inline bool has_step1() const;
-  inline void clear_step1();
-  static const int kStep1FieldNumber = 4;
-  inline ::google::protobuf::uint32 step1() const;
-  inline void set_step1(::google::protobuf::uint32 value);
-  
-  // required uint32 type1 = 5;
-  inline bool has_type1() const;
-  inline void clear_type1();
-  static const int kType1FieldNumber = 5;
-  inline ::google::protobuf::uint32 type1() const;
-  inline void set_type1(::google::protobuf::uint32 value);
-  
-  // required bytes imageData1 = 6;
-  inline bool has_imagedata1() const;
-  inline void clear_imagedata1();
-  static const int kImageData1FieldNumber = 6;
-  inline const ::std::string& imagedata1() const;
-  inline void set_imagedata1(const ::std::string& value);
-  inline void set_imagedata1(const char* value);
-  inline void set_imagedata1(const void* value, size_t size);
-  inline ::std::string* mutable_imagedata1();
-  inline ::std::string* release_imagedata1();
-  
-  // required uint32 step2 = 7;
-  inline bool has_step2() const;
-  inline void clear_step2();
-  static const int kStep2FieldNumber = 7;
-  inline ::google::protobuf::uint32 step2() const;
-  inline void set_step2(::google::protobuf::uint32 value);
-  
-  // required uint32 type2 = 8;
-  inline bool has_type2() const;
-  inline void clear_type2();
-  static const int kType2FieldNumber = 8;
-  inline ::google::protobuf::uint32 type2() const;
-  inline void set_type2(::google::protobuf::uint32 value);
-  
-  // required bytes imageData2 = 9;
-  inline bool has_imagedata2() const;
-  inline void clear_imagedata2();
-  static const int kImageData2FieldNumber = 9;
-  inline const ::std::string& imagedata2() const;
-  inline void set_imagedata2(const ::std::string& value);
-  inline void set_imagedata2(const char* value);
-  inline void set_imagedata2(const void* value, size_t size);
-  inline ::std::string* mutable_imagedata2();
-  inline ::std::string* release_imagedata2();
-  
-  // optional uint32 camera_config = 10;
-  inline bool has_camera_config() const;
-  inline void clear_camera_config();
-  static const int kCameraConfigFieldNumber = 10;
-  inline ::google::protobuf::uint32 camera_config() const;
-  inline void set_camera_config(::google::protobuf::uint32 value);
-  
-  // optional uint32 camera_type = 11;
-  inline bool has_camera_type() const;
-  inline void clear_camera_type();
-  static const int kCameraTypeFieldNumber = 11;
-  inline ::google::protobuf::uint32 camera_type() const;
-  inline void set_camera_type(::google::protobuf::uint32 value);
-  
-  // optional float roll = 12;
-  inline bool has_roll() const;
-  inline void clear_roll();
-  static const int kRollFieldNumber = 12;
-  inline float roll() const;
-  inline void set_roll(float value);
-  
-  // optional float pitch = 13;
-  inline bool has_pitch() const;
-  inline void clear_pitch();
-  static const int kPitchFieldNumber = 13;
-  inline float pitch() const;
-  inline void set_pitch(float value);
-  
-  // optional float yaw = 14;
-  inline bool has_yaw() const;
-  inline void clear_yaw();
-  static const int kYawFieldNumber = 14;
-  inline float yaw() const;
-  inline void set_yaw(float value);
-  
-  // optional float lon = 15;
-  inline bool has_lon() const;
-  inline void clear_lon();
-  static const int kLonFieldNumber = 15;
-  inline float lon() const;
-  inline void set_lon(float value);
-  
-  // optional float lat = 16;
-  inline bool has_lat() const;
-  inline void clear_lat();
-  static const int kLatFieldNumber = 16;
-  inline float lat() const;
-  inline void set_lat(float value);
-  
-  // optional float alt = 17;
-  inline bool has_alt() const;
-  inline void clear_alt();
-  static const int kAltFieldNumber = 17;
-  inline float alt() const;
-  inline void set_alt(float value);
-  
-  // optional float ground_x = 18;
-  inline bool has_ground_x() const;
-  inline void clear_ground_x();
-  static const int kGroundXFieldNumber = 18;
-  inline float ground_x() const;
-  inline void set_ground_x(float value);
-  
-  // optional float ground_y = 19;
-  inline bool has_ground_y() const;
-  inline void clear_ground_y();
-  static const int kGroundYFieldNumber = 19;
-  inline float ground_y() const;
-  inline void set_ground_y(float value);
-  
-  // optional float ground_z = 20;
-  inline bool has_ground_z() const;
-  inline void clear_ground_z();
-  static const int kGroundZFieldNumber = 20;
-  inline float ground_z() const;
-  inline void set_ground_z(float value);
-  
-  // repeated float camera_matrix = 21;
-  inline int camera_matrix_size() const;
-  inline void clear_camera_matrix();
-  static const int kCameraMatrixFieldNumber = 21;
-  inline float camera_matrix(int index) const;
-  inline void set_camera_matrix(int index, float value);
-  inline void add_camera_matrix(float value);
-  inline const ::google::protobuf::RepeatedField< float >&
-      camera_matrix() const;
-  inline ::google::protobuf::RepeatedField< float >*
-      mutable_camera_matrix();
-  
-  // @@protoc_insertion_point(class_scope:px.RGBDImage)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  inline void set_has_cols();
-  inline void clear_has_cols();
-  inline void set_has_rows();
-  inline void clear_has_rows();
-  inline void set_has_step1();
-  inline void clear_has_step1();
-  inline void set_has_type1();
-  inline void clear_has_type1();
-  inline void set_has_imagedata1();
-  inline void clear_has_imagedata1();
-  inline void set_has_step2();
-  inline void clear_has_step2();
-  inline void set_has_type2();
-  inline void clear_has_type2();
-  inline void set_has_imagedata2();
-  inline void clear_has_imagedata2();
-  inline void set_has_camera_config();
-  inline void clear_has_camera_config();
-  inline void set_has_camera_type();
-  inline void clear_has_camera_type();
-  inline void set_has_roll();
-  inline void clear_has_roll();
-  inline void set_has_pitch();
-  inline void clear_has_pitch();
-  inline void set_has_yaw();
-  inline void clear_has_yaw();
-  inline void set_has_lon();
-  inline void clear_has_lon();
-  inline void set_has_lat();
-  inline void clear_has_lat();
-  inline void set_has_alt();
-  inline void clear_has_alt();
-  inline void set_has_ground_x();
-  inline void clear_has_ground_x();
-  inline void set_has_ground_y();
-  inline void clear_has_ground_y();
-  inline void set_has_ground_z();
-  inline void clear_has_ground_z();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::uint32 cols_;
-  ::google::protobuf::uint32 rows_;
-  ::google::protobuf::uint32 step1_;
-  ::google::protobuf::uint32 type1_;
-  ::std::string* imagedata1_;
-  ::google::protobuf::uint32 step2_;
-  ::google::protobuf::uint32 type2_;
-  ::std::string* imagedata2_;
-  ::google::protobuf::uint32 camera_config_;
-  ::google::protobuf::uint32 camera_type_;
-  float roll_;
-  float pitch_;
-  float yaw_;
-  float lon_;
-  float lat_;
-  float alt_;
-  float ground_x_;
-  float ground_y_;
-  ::google::protobuf::RepeatedField< float > camera_matrix_;
-  float ground_z_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(21 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static RGBDImage* default_instance_;
-};
-// -------------------------------------------------------------------
-
-class Waypoint : public ::google::protobuf::Message {
- public:
-  Waypoint();
-  virtual ~Waypoint();
-  
-  Waypoint(const Waypoint& from);
-  
-  inline Waypoint& operator=(const Waypoint& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  
-  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
-    return _unknown_fields_;
-  }
-  
-  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
-    return &_unknown_fields_;
-  }
-  
-  static const ::google::protobuf::Descriptor* descriptor();
-  static const Waypoint& default_instance();
-  
-  void Swap(Waypoint* other);
-  
-  // implements Message ----------------------------------------------
-  
-  Waypoint* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const Waypoint& from);
-  void MergeFrom(const Waypoint& from);
-  void Clear();
-  bool IsInitialized() const;
-  
-  int ByteSize() const;
-  bool MergePartialFromCodedStream(
-      ::google::protobuf::io::CodedInputStream* input);
-  void SerializeWithCachedSizes(
-      ::google::protobuf::io::CodedOutputStream* output) const;
-  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
-  int GetCachedSize() const { return _cached_size_; }
-  private:
-  void SharedCtor();
-  void SharedDtor();
-  void SetCachedSize(int size) const;
-  public:
-  
-  ::google::protobuf::Metadata GetMetadata() const;
-  
-  // nested types ----------------------------------------------------
-  
-  // accessors -------------------------------------------------------
-  
-  // required double x = 1;
-  inline bool has_x() const;
-  inline void clear_x();
-  static const int kXFieldNumber = 1;
-  inline double x() const;
-  inline void set_x(double value);
-  
-  // required double y = 2;
-  inline bool has_y() const;
-  inline void clear_y();
-  static const int kYFieldNumber = 2;
-  inline double y() const;
-  inline void set_y(double value);
-  
-  // optional double z = 3;
-  inline bool has_z() const;
-  inline void clear_z();
-  static const int kZFieldNumber = 3;
-  inline double z() const;
-  inline void set_z(double value);
-  
-  // optional double roll = 4;
-  inline bool has_roll() const;
-  inline void clear_roll();
-  static const int kRollFieldNumber = 4;
-  inline double roll() const;
-  inline void set_roll(double value);
-  
-  // optional double pitch = 5;
-  inline bool has_pitch() const;
-  inline void clear_pitch();
-  static const int kPitchFieldNumber = 5;
-  inline double pitch() const;
-  inline void set_pitch(double value);
-  
-  // optional double yaw = 6;
-  inline bool has_yaw() const;
-  inline void clear_yaw();
-  static const int kYawFieldNumber = 6;
-  inline double yaw() const;
-  inline void set_yaw(double value);
-  
-  // @@protoc_insertion_point(class_scope:px.Waypoint)
- private:
-  inline void set_has_x();
-  inline void clear_has_x();
-  inline void set_has_y();
-  inline void clear_has_y();
-  inline void set_has_z();
-  inline void clear_has_z();
-  inline void set_has_roll();
-  inline void clear_has_roll();
-  inline void set_has_pitch();
-  inline void clear_has_pitch();
-  inline void set_has_yaw();
-  inline void clear_has_yaw();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  double x_;
-  double y_;
-  double z_;
-  double roll_;
-  double pitch_;
-  double yaw_;
-  
-  mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32];
-  
-  friend void  protobuf_AddDesc_pixhawk_2eproto();
-  friend void protobuf_AssignDesc_pixhawk_2eproto();
-  friend void protobuf_ShutdownFile_pixhawk_2eproto();
-  
-  void InitAsDefaultInstance();
-  static Waypoint* default_instance_;
-};
-// ===================================================================
-
-
-// ===================================================================
-
-// HeaderInfo
-
-// required int32 source_sysid = 1;
-inline bool HeaderInfo::has_source_sysid() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void HeaderInfo::set_has_source_sysid() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void HeaderInfo::clear_has_source_sysid() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void HeaderInfo::clear_source_sysid() {
-  source_sysid_ = 0;
-  clear_has_source_sysid();
-}
-inline ::google::protobuf::int32 HeaderInfo::source_sysid() const {
-  return source_sysid_;
-}
-inline void HeaderInfo::set_source_sysid(::google::protobuf::int32 value) {
-  set_has_source_sysid();
-  source_sysid_ = value;
-}
-
-// required int32 source_compid = 2;
-inline bool HeaderInfo::has_source_compid() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void HeaderInfo::set_has_source_compid() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void HeaderInfo::clear_has_source_compid() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void HeaderInfo::clear_source_compid() {
-  source_compid_ = 0;
-  clear_has_source_compid();
-}
-inline ::google::protobuf::int32 HeaderInfo::source_compid() const {
-  return source_compid_;
-}
-inline void HeaderInfo::set_source_compid(::google::protobuf::int32 value) {
-  set_has_source_compid();
-  source_compid_ = value;
-}
-
-// required double timestamp = 3;
-inline bool HeaderInfo::has_timestamp() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void HeaderInfo::set_has_timestamp() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void HeaderInfo::clear_has_timestamp() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void HeaderInfo::clear_timestamp() {
-  timestamp_ = 0;
-  clear_has_timestamp();
-}
-inline double HeaderInfo::timestamp() const {
-  return timestamp_;
-}
-inline void HeaderInfo::set_timestamp(double value) {
-  set_has_timestamp();
-  timestamp_ = value;
-}
-
-// -------------------------------------------------------------------
-
-// GLOverlay
-
-// required .px.HeaderInfo header = 1;
-inline bool GLOverlay::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void GLOverlay::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void GLOverlay::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void GLOverlay::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& GLOverlay::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* GLOverlay::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* GLOverlay::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// optional string name = 2;
-inline bool GLOverlay::has_name() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void GLOverlay::set_has_name() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void GLOverlay::clear_has_name() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void GLOverlay::clear_name() {
-  if (name_ != &::google::protobuf::internal::kEmptyString) {
-    name_->clear();
-  }
-  clear_has_name();
-}
-inline const ::std::string& GLOverlay::name() const {
-  return *name_;
-}
-inline void GLOverlay::set_name(const ::std::string& value) {
-  set_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    name_ = new ::std::string;
-  }
-  name_->assign(value);
-}
-inline void GLOverlay::set_name(const char* value) {
-  set_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    name_ = new ::std::string;
-  }
-  name_->assign(value);
-}
-inline void GLOverlay::set_name(const char* value, size_t size) {
-  set_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    name_ = new ::std::string;
-  }
-  name_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* GLOverlay::mutable_name() {
-  set_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    name_ = new ::std::string;
-  }
-  return name_;
-}
-inline ::std::string* GLOverlay::release_name() {
-  clear_has_name();
-  if (name_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = name_;
-    name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-inline bool GLOverlay::has_coordinateframetype() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void GLOverlay::set_has_coordinateframetype() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void GLOverlay::clear_has_coordinateframetype() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void GLOverlay::clear_coordinateframetype() {
-  coordinateframetype_ = 0;
-  clear_has_coordinateframetype();
-}
-inline ::px::GLOverlay_CoordinateFrameType GLOverlay::coordinateframetype() const {
-  return static_cast< ::px::GLOverlay_CoordinateFrameType >(coordinateframetype_);
-}
-inline void GLOverlay::set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value) {
-  GOOGLE_DCHECK(::px::GLOverlay_CoordinateFrameType_IsValid(value));
-  set_has_coordinateframetype();
-  coordinateframetype_ = value;
-}
-
-// optional double origin_x = 4;
-inline bool GLOverlay::has_origin_x() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void GLOverlay::set_has_origin_x() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void GLOverlay::clear_has_origin_x() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void GLOverlay::clear_origin_x() {
-  origin_x_ = 0;
-  clear_has_origin_x();
-}
-inline double GLOverlay::origin_x() const {
-  return origin_x_;
-}
-inline void GLOverlay::set_origin_x(double value) {
-  set_has_origin_x();
-  origin_x_ = value;
-}
-
-// optional double origin_y = 5;
-inline bool GLOverlay::has_origin_y() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void GLOverlay::set_has_origin_y() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void GLOverlay::clear_has_origin_y() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void GLOverlay::clear_origin_y() {
-  origin_y_ = 0;
-  clear_has_origin_y();
-}
-inline double GLOverlay::origin_y() const {
-  return origin_y_;
-}
-inline void GLOverlay::set_origin_y(double value) {
-  set_has_origin_y();
-  origin_y_ = value;
-}
-
-// optional double origin_z = 6;
-inline bool GLOverlay::has_origin_z() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void GLOverlay::set_has_origin_z() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void GLOverlay::clear_has_origin_z() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void GLOverlay::clear_origin_z() {
-  origin_z_ = 0;
-  clear_has_origin_z();
-}
-inline double GLOverlay::origin_z() const {
-  return origin_z_;
-}
-inline void GLOverlay::set_origin_z(double value) {
-  set_has_origin_z();
-  origin_z_ = value;
-}
-
-// optional bytes data = 7;
-inline bool GLOverlay::has_data() const {
-  return (_has_bits_[0] & 0x00000040u) != 0;
-}
-inline void GLOverlay::set_has_data() {
-  _has_bits_[0] |= 0x00000040u;
-}
-inline void GLOverlay::clear_has_data() {
-  _has_bits_[0] &= ~0x00000040u;
-}
-inline void GLOverlay::clear_data() {
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    data_->clear();
-  }
-  clear_has_data();
-}
-inline const ::std::string& GLOverlay::data() const {
-  return *data_;
-}
-inline void GLOverlay::set_data(const ::std::string& value) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(value);
-}
-inline void GLOverlay::set_data(const char* value) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(value);
-}
-inline void GLOverlay::set_data(const void* value, size_t size) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* GLOverlay::mutable_data() {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  return data_;
-}
-inline ::std::string* GLOverlay::release_data() {
-  clear_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = data_;
-    data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// -------------------------------------------------------------------
-
-// Obstacle
-
-// optional float x = 1;
-inline bool Obstacle::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void Obstacle::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void Obstacle::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void Obstacle::clear_x() {
-  x_ = 0;
-  clear_has_x();
-}
-inline float Obstacle::x() const {
-  return x_;
-}
-inline void Obstacle::set_x(float value) {
-  set_has_x();
-  x_ = value;
-}
-
-// optional float y = 2;
-inline bool Obstacle::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void Obstacle::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void Obstacle::clear_has_y() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void Obstacle::clear_y() {
-  y_ = 0;
-  clear_has_y();
-}
-inline float Obstacle::y() const {
-  return y_;
-}
-inline void Obstacle::set_y(float value) {
-  set_has_y();
-  y_ = value;
-}
-
-// optional float z = 3;
-inline bool Obstacle::has_z() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void Obstacle::set_has_z() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void Obstacle::clear_has_z() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void Obstacle::clear_z() {
-  z_ = 0;
-  clear_has_z();
-}
-inline float Obstacle::z() const {
-  return z_;
-}
-inline void Obstacle::set_z(float value) {
-  set_has_z();
-  z_ = value;
-}
-
-// optional float length = 4;
-inline bool Obstacle::has_length() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void Obstacle::set_has_length() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void Obstacle::clear_has_length() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void Obstacle::clear_length() {
-  length_ = 0;
-  clear_has_length();
-}
-inline float Obstacle::length() const {
-  return length_;
-}
-inline void Obstacle::set_length(float value) {
-  set_has_length();
-  length_ = value;
-}
-
-// optional float width = 5;
-inline bool Obstacle::has_width() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void Obstacle::set_has_width() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void Obstacle::clear_has_width() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void Obstacle::clear_width() {
-  width_ = 0;
-  clear_has_width();
-}
-inline float Obstacle::width() const {
-  return width_;
-}
-inline void Obstacle::set_width(float value) {
-  set_has_width();
-  width_ = value;
-}
-
-// optional float height = 6;
-inline bool Obstacle::has_height() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void Obstacle::set_has_height() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void Obstacle::clear_has_height() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void Obstacle::clear_height() {
-  height_ = 0;
-  clear_has_height();
-}
-inline float Obstacle::height() const {
-  return height_;
-}
-inline void Obstacle::set_height(float value) {
-  set_has_height();
-  height_ = value;
-}
-
-// -------------------------------------------------------------------
-
-// ObstacleList
-
-// required .px.HeaderInfo header = 1;
-inline bool ObstacleList::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void ObstacleList::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void ObstacleList::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void ObstacleList::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& ObstacleList::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* ObstacleList::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* ObstacleList::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.Obstacle obstacles = 2;
-inline int ObstacleList::obstacles_size() const {
-  return obstacles_.size();
-}
-inline void ObstacleList::clear_obstacles() {
-  obstacles_.Clear();
-}
-inline const ::px::Obstacle& ObstacleList::obstacles(int index) const {
-  return obstacles_.Get(index);
-}
-inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) {
-  return obstacles_.Mutable(index);
-}
-inline ::px::Obstacle* ObstacleList::add_obstacles() {
-  return obstacles_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
-ObstacleList::obstacles() const {
-  return obstacles_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
-ObstacleList::mutable_obstacles() {
-  return &obstacles_;
-}
-
-// -------------------------------------------------------------------
-
-// ObstacleMap
-
-// required .px.HeaderInfo header = 1;
-inline bool ObstacleMap::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void ObstacleMap::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void ObstacleMap::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void ObstacleMap::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& ObstacleMap::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* ObstacleMap::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* ObstacleMap::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// required int32 type = 2;
-inline bool ObstacleMap::has_type() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void ObstacleMap::set_has_type() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void ObstacleMap::clear_has_type() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void ObstacleMap::clear_type() {
-  type_ = 0;
-  clear_has_type();
-}
-inline ::google::protobuf::int32 ObstacleMap::type() const {
-  return type_;
-}
-inline void ObstacleMap::set_type(::google::protobuf::int32 value) {
-  set_has_type();
-  type_ = value;
-}
-
-// optional float resolution = 3;
-inline bool ObstacleMap::has_resolution() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void ObstacleMap::set_has_resolution() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void ObstacleMap::clear_has_resolution() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void ObstacleMap::clear_resolution() {
-  resolution_ = 0;
-  clear_has_resolution();
-}
-inline float ObstacleMap::resolution() const {
-  return resolution_;
-}
-inline void ObstacleMap::set_resolution(float value) {
-  set_has_resolution();
-  resolution_ = value;
-}
-
-// optional int32 rows = 4;
-inline bool ObstacleMap::has_rows() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void ObstacleMap::set_has_rows() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void ObstacleMap::clear_has_rows() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void ObstacleMap::clear_rows() {
-  rows_ = 0;
-  clear_has_rows();
-}
-inline ::google::protobuf::int32 ObstacleMap::rows() const {
-  return rows_;
-}
-inline void ObstacleMap::set_rows(::google::protobuf::int32 value) {
-  set_has_rows();
-  rows_ = value;
-}
-
-// optional int32 cols = 5;
-inline bool ObstacleMap::has_cols() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void ObstacleMap::set_has_cols() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void ObstacleMap::clear_has_cols() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void ObstacleMap::clear_cols() {
-  cols_ = 0;
-  clear_has_cols();
-}
-inline ::google::protobuf::int32 ObstacleMap::cols() const {
-  return cols_;
-}
-inline void ObstacleMap::set_cols(::google::protobuf::int32 value) {
-  set_has_cols();
-  cols_ = value;
-}
-
-// optional int32 mapR0 = 6;
-inline bool ObstacleMap::has_mapr0() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void ObstacleMap::set_has_mapr0() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void ObstacleMap::clear_has_mapr0() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void ObstacleMap::clear_mapr0() {
-  mapr0_ = 0;
-  clear_has_mapr0();
-}
-inline ::google::protobuf::int32 ObstacleMap::mapr0() const {
-  return mapr0_;
-}
-inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) {
-  set_has_mapr0();
-  mapr0_ = value;
-}
-
-// optional int32 mapC0 = 7;
-inline bool ObstacleMap::has_mapc0() const {
-  return (_has_bits_[0] & 0x00000040u) != 0;
-}
-inline void ObstacleMap::set_has_mapc0() {
-  _has_bits_[0] |= 0x00000040u;
-}
-inline void ObstacleMap::clear_has_mapc0() {
-  _has_bits_[0] &= ~0x00000040u;
-}
-inline void ObstacleMap::clear_mapc0() {
-  mapc0_ = 0;
-  clear_has_mapc0();
-}
-inline ::google::protobuf::int32 ObstacleMap::mapc0() const {
-  return mapc0_;
-}
-inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) {
-  set_has_mapc0();
-  mapc0_ = value;
-}
-
-// optional int32 arrayR0 = 8;
-inline bool ObstacleMap::has_arrayr0() const {
-  return (_has_bits_[0] & 0x00000080u) != 0;
-}
-inline void ObstacleMap::set_has_arrayr0() {
-  _has_bits_[0] |= 0x00000080u;
-}
-inline void ObstacleMap::clear_has_arrayr0() {
-  _has_bits_[0] &= ~0x00000080u;
-}
-inline void ObstacleMap::clear_arrayr0() {
-  arrayr0_ = 0;
-  clear_has_arrayr0();
-}
-inline ::google::protobuf::int32 ObstacleMap::arrayr0() const {
-  return arrayr0_;
-}
-inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) {
-  set_has_arrayr0();
-  arrayr0_ = value;
-}
-
-// optional int32 arrayC0 = 9;
-inline bool ObstacleMap::has_arrayc0() const {
-  return (_has_bits_[0] & 0x00000100u) != 0;
-}
-inline void ObstacleMap::set_has_arrayc0() {
-  _has_bits_[0] |= 0x00000100u;
-}
-inline void ObstacleMap::clear_has_arrayc0() {
-  _has_bits_[0] &= ~0x00000100u;
-}
-inline void ObstacleMap::clear_arrayc0() {
-  arrayc0_ = 0;
-  clear_has_arrayc0();
-}
-inline ::google::protobuf::int32 ObstacleMap::arrayc0() const {
-  return arrayc0_;
-}
-inline void ObstacleMap::set_arrayc0(::google::protobuf::int32 value) {
-  set_has_arrayc0();
-  arrayc0_ = value;
-}
-
-// optional bytes data = 10;
-inline bool ObstacleMap::has_data() const {
-  return (_has_bits_[0] & 0x00000200u) != 0;
-}
-inline void ObstacleMap::set_has_data() {
-  _has_bits_[0] |= 0x00000200u;
-}
-inline void ObstacleMap::clear_has_data() {
-  _has_bits_[0] &= ~0x00000200u;
-}
-inline void ObstacleMap::clear_data() {
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    data_->clear();
-  }
-  clear_has_data();
-}
-inline const ::std::string& ObstacleMap::data() const {
-  return *data_;
-}
-inline void ObstacleMap::set_data(const ::std::string& value) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(value);
-}
-inline void ObstacleMap::set_data(const char* value) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(value);
-}
-inline void ObstacleMap::set_data(const void* value, size_t size) {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  data_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* ObstacleMap::mutable_data() {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  return data_;
-}
-inline ::std::string* ObstacleMap::release_data() {
-  clear_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = data_;
-    data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// -------------------------------------------------------------------
-
-// Path
-
-// required .px.HeaderInfo header = 1;
-inline bool Path::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void Path::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void Path::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void Path::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& Path::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* Path::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* Path::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.Waypoint waypoints = 2;
-inline int Path::waypoints_size() const {
-  return waypoints_.size();
-}
-inline void Path::clear_waypoints() {
-  waypoints_.Clear();
-}
-inline const ::px::Waypoint& Path::waypoints(int index) const {
-  return waypoints_.Get(index);
-}
-inline ::px::Waypoint* Path::mutable_waypoints(int index) {
-  return waypoints_.Mutable(index);
-}
-inline ::px::Waypoint* Path::add_waypoints() {
-  return waypoints_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >&
-Path::waypoints() const {
-  return waypoints_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >*
-Path::mutable_waypoints() {
-  return &waypoints_;
-}
-
-// -------------------------------------------------------------------
-
-// PointCloudXYZI_PointXYZI
-
-// required float x = 1;
-inline bool PointCloudXYZI_PointXYZI::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZI_PointXYZI::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_x() {
-  x_ = 0;
-  clear_has_x();
-}
-inline float PointCloudXYZI_PointXYZI::x() const {
-  return x_;
-}
-inline void PointCloudXYZI_PointXYZI::set_x(float value) {
-  set_has_x();
-  x_ = value;
-}
-
-// required float y = 2;
-inline bool PointCloudXYZI_PointXYZI::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void PointCloudXYZI_PointXYZI::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_has_y() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_y() {
-  y_ = 0;
-  clear_has_y();
-}
-inline float PointCloudXYZI_PointXYZI::y() const {
-  return y_;
-}
-inline void PointCloudXYZI_PointXYZI::set_y(float value) {
-  set_has_y();
-  y_ = value;
-}
-
-// required float z = 3;
-inline bool PointCloudXYZI_PointXYZI::has_z() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void PointCloudXYZI_PointXYZI::set_has_z() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_has_z() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_z() {
-  z_ = 0;
-  clear_has_z();
-}
-inline float PointCloudXYZI_PointXYZI::z() const {
-  return z_;
-}
-inline void PointCloudXYZI_PointXYZI::set_z(float value) {
-  set_has_z();
-  z_ = value;
-}
-
-// required float intensity = 4;
-inline bool PointCloudXYZI_PointXYZI::has_intensity() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void PointCloudXYZI_PointXYZI::set_has_intensity() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_has_intensity() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void PointCloudXYZI_PointXYZI::clear_intensity() {
-  intensity_ = 0;
-  clear_has_intensity();
-}
-inline float PointCloudXYZI_PointXYZI::intensity() const {
-  return intensity_;
-}
-inline void PointCloudXYZI_PointXYZI::set_intensity(float value) {
-  set_has_intensity();
-  intensity_ = value;
-}
-
-// -------------------------------------------------------------------
-
-// PointCloudXYZI
-
-// required .px.HeaderInfo header = 1;
-inline bool PointCloudXYZI::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZI::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void PointCloudXYZI::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void PointCloudXYZI::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& PointCloudXYZI::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* PointCloudXYZI::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* PointCloudXYZI::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.PointCloudXYZI.PointXYZI points = 2;
-inline int PointCloudXYZI::points_size() const {
-  return points_.size();
-}
-inline void PointCloudXYZI::clear_points() {
-  points_.Clear();
-}
-inline const ::px::PointCloudXYZI_PointXYZI& PointCloudXYZI::points(int index) const {
-  return points_.Get(index);
-}
-inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::mutable_points(int index) {
-  return points_.Mutable(index);
-}
-inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::add_points() {
-  return points_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >&
-PointCloudXYZI::points() const {
-  return points_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >*
-PointCloudXYZI::mutable_points() {
-  return &points_;
-}
-
-// -------------------------------------------------------------------
-
-// PointCloudXYZRGB_PointXYZRGB
-
-// required float x = 1;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_x() {
-  x_ = 0;
-  clear_has_x();
-}
-inline float PointCloudXYZRGB_PointXYZRGB::x() const {
-  return x_;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_x(float value) {
-  set_has_x();
-  x_ = value;
-}
-
-// required float y = 2;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_y() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_y() {
-  y_ = 0;
-  clear_has_y();
-}
-inline float PointCloudXYZRGB_PointXYZRGB::y() const {
-  return y_;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_y(float value) {
-  set_has_y();
-  y_ = value;
-}
-
-// required float z = 3;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_z() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_z() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_z() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_z() {
-  z_ = 0;
-  clear_has_z();
-}
-inline float PointCloudXYZRGB_PointXYZRGB::z() const {
-  return z_;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_z(float value) {
-  set_has_z();
-  z_ = value;
-}
-
-// required float rgb = 4;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_rgb() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_rgb() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_rgb() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::clear_rgb() {
-  rgb_ = 0;
-  clear_has_rgb();
-}
-inline float PointCloudXYZRGB_PointXYZRGB::rgb() const {
-  return rgb_;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_rgb(float value) {
-  set_has_rgb();
-  rgb_ = value;
-}
-
-// -------------------------------------------------------------------
-
-// PointCloudXYZRGB
-
-// required .px.HeaderInfo header = 1;
-inline bool PointCloudXYZRGB::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZRGB::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void PointCloudXYZRGB::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void PointCloudXYZRGB::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& PointCloudXYZRGB::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* PointCloudXYZRGB::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* PointCloudXYZRGB::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-inline int PointCloudXYZRGB::points_size() const {
-  return points_.size();
-}
-inline void PointCloudXYZRGB::clear_points() {
-  points_.Clear();
-}
-inline const ::px::PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB::points(int index) const {
-  return points_.Get(index);
-}
-inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::mutable_points(int index) {
-  return points_.Mutable(index);
-}
-inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::add_points() {
-  return points_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >&
-PointCloudXYZRGB::points() const {
-  return points_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >*
-PointCloudXYZRGB::mutable_points() {
-  return &points_;
-}
-
-// -------------------------------------------------------------------
-
-// RGBDImage
-
-// required .px.HeaderInfo header = 1;
-inline bool RGBDImage::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void RGBDImage::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void RGBDImage::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void RGBDImage::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& RGBDImage::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* RGBDImage::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* RGBDImage::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// required uint32 cols = 2;
-inline bool RGBDImage::has_cols() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void RGBDImage::set_has_cols() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void RGBDImage::clear_has_cols() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void RGBDImage::clear_cols() {
-  cols_ = 0u;
-  clear_has_cols();
-}
-inline ::google::protobuf::uint32 RGBDImage::cols() const {
-  return cols_;
-}
-inline void RGBDImage::set_cols(::google::protobuf::uint32 value) {
-  set_has_cols();
-  cols_ = value;
-}
-
-// required uint32 rows = 3;
-inline bool RGBDImage::has_rows() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void RGBDImage::set_has_rows() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void RGBDImage::clear_has_rows() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void RGBDImage::clear_rows() {
-  rows_ = 0u;
-  clear_has_rows();
-}
-inline ::google::protobuf::uint32 RGBDImage::rows() const {
-  return rows_;
-}
-inline void RGBDImage::set_rows(::google::protobuf::uint32 value) {
-  set_has_rows();
-  rows_ = value;
-}
-
-// required uint32 step1 = 4;
-inline bool RGBDImage::has_step1() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void RGBDImage::set_has_step1() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void RGBDImage::clear_has_step1() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void RGBDImage::clear_step1() {
-  step1_ = 0u;
-  clear_has_step1();
-}
-inline ::google::protobuf::uint32 RGBDImage::step1() const {
-  return step1_;
-}
-inline void RGBDImage::set_step1(::google::protobuf::uint32 value) {
-  set_has_step1();
-  step1_ = value;
-}
-
-// required uint32 type1 = 5;
-inline bool RGBDImage::has_type1() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void RGBDImage::set_has_type1() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void RGBDImage::clear_has_type1() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void RGBDImage::clear_type1() {
-  type1_ = 0u;
-  clear_has_type1();
-}
-inline ::google::protobuf::uint32 RGBDImage::type1() const {
-  return type1_;
-}
-inline void RGBDImage::set_type1(::google::protobuf::uint32 value) {
-  set_has_type1();
-  type1_ = value;
-}
-
-// required bytes imageData1 = 6;
-inline bool RGBDImage::has_imagedata1() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void RGBDImage::set_has_imagedata1() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void RGBDImage::clear_has_imagedata1() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void RGBDImage::clear_imagedata1() {
-  if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-    imagedata1_->clear();
-  }
-  clear_has_imagedata1();
-}
-inline const ::std::string& RGBDImage::imagedata1() const {
-  return *imagedata1_;
-}
-inline void RGBDImage::set_imagedata1(const ::std::string& value) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(value);
-}
-inline void RGBDImage::set_imagedata1(const char* value) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(value);
-}
-inline void RGBDImage::set_imagedata1(const void* value, size_t size) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* RGBDImage::mutable_imagedata1() {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  return imagedata1_;
-}
-inline ::std::string* RGBDImage::release_imagedata1() {
-  clear_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = imagedata1_;
-    imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// required uint32 step2 = 7;
-inline bool RGBDImage::has_step2() const {
-  return (_has_bits_[0] & 0x00000040u) != 0;
-}
-inline void RGBDImage::set_has_step2() {
-  _has_bits_[0] |= 0x00000040u;
-}
-inline void RGBDImage::clear_has_step2() {
-  _has_bits_[0] &= ~0x00000040u;
-}
-inline void RGBDImage::clear_step2() {
-  step2_ = 0u;
-  clear_has_step2();
-}
-inline ::google::protobuf::uint32 RGBDImage::step2() const {
-  return step2_;
-}
-inline void RGBDImage::set_step2(::google::protobuf::uint32 value) {
-  set_has_step2();
-  step2_ = value;
-}
-
-// required uint32 type2 = 8;
-inline bool RGBDImage::has_type2() const {
-  return (_has_bits_[0] & 0x00000080u) != 0;
-}
-inline void RGBDImage::set_has_type2() {
-  _has_bits_[0] |= 0x00000080u;
-}
-inline void RGBDImage::clear_has_type2() {
-  _has_bits_[0] &= ~0x00000080u;
-}
-inline void RGBDImage::clear_type2() {
-  type2_ = 0u;
-  clear_has_type2();
-}
-inline ::google::protobuf::uint32 RGBDImage::type2() const {
-  return type2_;
-}
-inline void RGBDImage::set_type2(::google::protobuf::uint32 value) {
-  set_has_type2();
-  type2_ = value;
-}
-
-// required bytes imageData2 = 9;
-inline bool RGBDImage::has_imagedata2() const {
-  return (_has_bits_[0] & 0x00000100u) != 0;
-}
-inline void RGBDImage::set_has_imagedata2() {
-  _has_bits_[0] |= 0x00000100u;
-}
-inline void RGBDImage::clear_has_imagedata2() {
-  _has_bits_[0] &= ~0x00000100u;
-}
-inline void RGBDImage::clear_imagedata2() {
-  if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-    imagedata2_->clear();
-  }
-  clear_has_imagedata2();
-}
-inline const ::std::string& RGBDImage::imagedata2() const {
-  return *imagedata2_;
-}
-inline void RGBDImage::set_imagedata2(const ::std::string& value) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
-  }
-  imagedata2_->assign(value);
-}
-inline void RGBDImage::set_imagedata2(const char* value) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
-  }
-  imagedata2_->assign(value);
-}
-inline void RGBDImage::set_imagedata2(const void* value, size_t size) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
-  }
-  imagedata2_->assign(reinterpret_cast<const char*>(value), size);
-}
-inline ::std::string* RGBDImage::mutable_imagedata2() {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
-  }
-  return imagedata2_;
-}
-inline ::std::string* RGBDImage::release_imagedata2() {
-  clear_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = imagedata2_;
-    imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
-}
-
-// optional uint32 camera_config = 10;
-inline bool RGBDImage::has_camera_config() const {
-  return (_has_bits_[0] & 0x00000200u) != 0;
-}
-inline void RGBDImage::set_has_camera_config() {
-  _has_bits_[0] |= 0x00000200u;
-}
-inline void RGBDImage::clear_has_camera_config() {
-  _has_bits_[0] &= ~0x00000200u;
-}
-inline void RGBDImage::clear_camera_config() {
-  camera_config_ = 0u;
-  clear_has_camera_config();
-}
-inline ::google::protobuf::uint32 RGBDImage::camera_config() const {
-  return camera_config_;
-}
-inline void RGBDImage::set_camera_config(::google::protobuf::uint32 value) {
-  set_has_camera_config();
-  camera_config_ = value;
-}
-
-// optional uint32 camera_type = 11;
-inline bool RGBDImage::has_camera_type() const {
-  return (_has_bits_[0] & 0x00000400u) != 0;
-}
-inline void RGBDImage::set_has_camera_type() {
-  _has_bits_[0] |= 0x00000400u;
-}
-inline void RGBDImage::clear_has_camera_type() {
-  _has_bits_[0] &= ~0x00000400u;
-}
-inline void RGBDImage::clear_camera_type() {
-  camera_type_ = 0u;
-  clear_has_camera_type();
-}
-inline ::google::protobuf::uint32 RGBDImage::camera_type() const {
-  return camera_type_;
-}
-inline void RGBDImage::set_camera_type(::google::protobuf::uint32 value) {
-  set_has_camera_type();
-  camera_type_ = value;
-}
-
-// optional float roll = 12;
-inline bool RGBDImage::has_roll() const {
-  return (_has_bits_[0] & 0x00000800u) != 0;
-}
-inline void RGBDImage::set_has_roll() {
-  _has_bits_[0] |= 0x00000800u;
-}
-inline void RGBDImage::clear_has_roll() {
-  _has_bits_[0] &= ~0x00000800u;
-}
-inline void RGBDImage::clear_roll() {
-  roll_ = 0;
-  clear_has_roll();
-}
-inline float RGBDImage::roll() const {
-  return roll_;
-}
-inline void RGBDImage::set_roll(float value) {
-  set_has_roll();
-  roll_ = value;
-}
-
-// optional float pitch = 13;
-inline bool RGBDImage::has_pitch() const {
-  return (_has_bits_[0] & 0x00001000u) != 0;
-}
-inline void RGBDImage::set_has_pitch() {
-  _has_bits_[0] |= 0x00001000u;
-}
-inline void RGBDImage::clear_has_pitch() {
-  _has_bits_[0] &= ~0x00001000u;
-}
-inline void RGBDImage::clear_pitch() {
-  pitch_ = 0;
-  clear_has_pitch();
-}
-inline float RGBDImage::pitch() const {
-  return pitch_;
-}
-inline void RGBDImage::set_pitch(float value) {
-  set_has_pitch();
-  pitch_ = value;
-}
-
-// optional float yaw = 14;
-inline bool RGBDImage::has_yaw() const {
-  return (_has_bits_[0] & 0x00002000u) != 0;
-}
-inline void RGBDImage::set_has_yaw() {
-  _has_bits_[0] |= 0x00002000u;
-}
-inline void RGBDImage::clear_has_yaw() {
-  _has_bits_[0] &= ~0x00002000u;
-}
-inline void RGBDImage::clear_yaw() {
-  yaw_ = 0;
-  clear_has_yaw();
-}
-inline float RGBDImage::yaw() const {
-  return yaw_;
-}
-inline void RGBDImage::set_yaw(float value) {
-  set_has_yaw();
-  yaw_ = value;
-}
-
-// optional float lon = 15;
-inline bool RGBDImage::has_lon() const {
-  return (_has_bits_[0] & 0x00004000u) != 0;
-}
-inline void RGBDImage::set_has_lon() {
-  _has_bits_[0] |= 0x00004000u;
-}
-inline void RGBDImage::clear_has_lon() {
-  _has_bits_[0] &= ~0x00004000u;
-}
-inline void RGBDImage::clear_lon() {
-  lon_ = 0;
-  clear_has_lon();
-}
-inline float RGBDImage::lon() const {
-  return lon_;
-}
-inline void RGBDImage::set_lon(float value) {
-  set_has_lon();
-  lon_ = value;
-}
-
-// optional float lat = 16;
-inline bool RGBDImage::has_lat() const {
-  return (_has_bits_[0] & 0x00008000u) != 0;
-}
-inline void RGBDImage::set_has_lat() {
-  _has_bits_[0] |= 0x00008000u;
-}
-inline void RGBDImage::clear_has_lat() {
-  _has_bits_[0] &= ~0x00008000u;
-}
-inline void RGBDImage::clear_lat() {
-  lat_ = 0;
-  clear_has_lat();
-}
-inline float RGBDImage::lat() const {
-  return lat_;
-}
-inline void RGBDImage::set_lat(float value) {
-  set_has_lat();
-  lat_ = value;
-}
-
-// optional float alt = 17;
-inline bool RGBDImage::has_alt() const {
-  return (_has_bits_[0] & 0x00010000u) != 0;
-}
-inline void RGBDImage::set_has_alt() {
-  _has_bits_[0] |= 0x00010000u;
-}
-inline void RGBDImage::clear_has_alt() {
-  _has_bits_[0] &= ~0x00010000u;
-}
-inline void RGBDImage::clear_alt() {
-  alt_ = 0;
-  clear_has_alt();
-}
-inline float RGBDImage::alt() const {
-  return alt_;
-}
-inline void RGBDImage::set_alt(float value) {
-  set_has_alt();
-  alt_ = value;
-}
-
-// optional float ground_x = 18;
-inline bool RGBDImage::has_ground_x() const {
-  return (_has_bits_[0] & 0x00020000u) != 0;
-}
-inline void RGBDImage::set_has_ground_x() {
-  _has_bits_[0] |= 0x00020000u;
-}
-inline void RGBDImage::clear_has_ground_x() {
-  _has_bits_[0] &= ~0x00020000u;
-}
-inline void RGBDImage::clear_ground_x() {
-  ground_x_ = 0;
-  clear_has_ground_x();
-}
-inline float RGBDImage::ground_x() const {
-  return ground_x_;
-}
-inline void RGBDImage::set_ground_x(float value) {
-  set_has_ground_x();
-  ground_x_ = value;
-}
-
-// optional float ground_y = 19;
-inline bool RGBDImage::has_ground_y() const {
-  return (_has_bits_[0] & 0x00040000u) != 0;
-}
-inline void RGBDImage::set_has_ground_y() {
-  _has_bits_[0] |= 0x00040000u;
-}
-inline void RGBDImage::clear_has_ground_y() {
-  _has_bits_[0] &= ~0x00040000u;
-}
-inline void RGBDImage::clear_ground_y() {
-  ground_y_ = 0;
-  clear_has_ground_y();
-}
-inline float RGBDImage::ground_y() const {
-  return ground_y_;
-}
-inline void RGBDImage::set_ground_y(float value) {
-  set_has_ground_y();
-  ground_y_ = value;
-}
-
-// optional float ground_z = 20;
-inline bool RGBDImage::has_ground_z() const {
-  return (_has_bits_[0] & 0x00080000u) != 0;
-}
-inline void RGBDImage::set_has_ground_z() {
-  _has_bits_[0] |= 0x00080000u;
-}
-inline void RGBDImage::clear_has_ground_z() {
-  _has_bits_[0] &= ~0x00080000u;
-}
-inline void RGBDImage::clear_ground_z() {
-  ground_z_ = 0;
-  clear_has_ground_z();
-}
-inline float RGBDImage::ground_z() const {
-  return ground_z_;
-}
-inline void RGBDImage::set_ground_z(float value) {
-  set_has_ground_z();
-  ground_z_ = value;
-}
-
-// repeated float camera_matrix = 21;
-inline int RGBDImage::camera_matrix_size() const {
-  return camera_matrix_.size();
-}
-inline void RGBDImage::clear_camera_matrix() {
-  camera_matrix_.Clear();
-}
-inline float RGBDImage::camera_matrix(int index) const {
-  return camera_matrix_.Get(index);
-}
-inline void RGBDImage::set_camera_matrix(int index, float value) {
-  camera_matrix_.Set(index, value);
-}
-inline void RGBDImage::add_camera_matrix(float value) {
-  camera_matrix_.Add(value);
-}
-inline const ::google::protobuf::RepeatedField< float >&
-RGBDImage::camera_matrix() const {
-  return camera_matrix_;
-}
-inline ::google::protobuf::RepeatedField< float >*
-RGBDImage::mutable_camera_matrix() {
-  return &camera_matrix_;
-}
-
-// -------------------------------------------------------------------
-
-// Waypoint
-
-// required double x = 1;
-inline bool Waypoint::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void Waypoint::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void Waypoint::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void Waypoint::clear_x() {
-  x_ = 0;
-  clear_has_x();
-}
-inline double Waypoint::x() const {
-  return x_;
-}
-inline void Waypoint::set_x(double value) {
-  set_has_x();
-  x_ = value;
-}
-
-// required double y = 2;
-inline bool Waypoint::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-inline void Waypoint::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
-}
-inline void Waypoint::clear_has_y() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline void Waypoint::clear_y() {
-  y_ = 0;
-  clear_has_y();
-}
-inline double Waypoint::y() const {
-  return y_;
-}
-inline void Waypoint::set_y(double value) {
-  set_has_y();
-  y_ = value;
-}
-
-// optional double z = 3;
-inline bool Waypoint::has_z() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-inline void Waypoint::set_has_z() {
-  _has_bits_[0] |= 0x00000004u;
-}
-inline void Waypoint::clear_has_z() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline void Waypoint::clear_z() {
-  z_ = 0;
-  clear_has_z();
-}
-inline double Waypoint::z() const {
-  return z_;
-}
-inline void Waypoint::set_z(double value) {
-  set_has_z();
-  z_ = value;
-}
-
-// optional double roll = 4;
-inline bool Waypoint::has_roll() const {
-  return (_has_bits_[0] & 0x00000008u) != 0;
-}
-inline void Waypoint::set_has_roll() {
-  _has_bits_[0] |= 0x00000008u;
-}
-inline void Waypoint::clear_has_roll() {
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline void Waypoint::clear_roll() {
-  roll_ = 0;
-  clear_has_roll();
-}
-inline double Waypoint::roll() const {
-  return roll_;
-}
-inline void Waypoint::set_roll(double value) {
-  set_has_roll();
-  roll_ = value;
-}
-
-// optional double pitch = 5;
-inline bool Waypoint::has_pitch() const {
-  return (_has_bits_[0] & 0x00000010u) != 0;
-}
-inline void Waypoint::set_has_pitch() {
-  _has_bits_[0] |= 0x00000010u;
-}
-inline void Waypoint::clear_has_pitch() {
-  _has_bits_[0] &= ~0x00000010u;
-}
-inline void Waypoint::clear_pitch() {
-  pitch_ = 0;
-  clear_has_pitch();
-}
-inline double Waypoint::pitch() const {
-  return pitch_;
-}
-inline void Waypoint::set_pitch(double value) {
-  set_has_pitch();
-  pitch_ = value;
-}
-
-// optional double yaw = 6;
-inline bool Waypoint::has_yaw() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
-}
-inline void Waypoint::set_has_yaw() {
-  _has_bits_[0] |= 0x00000020u;
-}
-inline void Waypoint::clear_has_yaw() {
-  _has_bits_[0] &= ~0x00000020u;
-}
-inline void Waypoint::clear_yaw() {
-  yaw_ = 0;
-  clear_has_yaw();
-}
-inline double Waypoint::yaw() const {
-  return yaw_;
-}
-inline void Waypoint::set_yaw(double value) {
-  set_has_yaw();
-  yaw_ = value;
-}
-
-
-// @@protoc_insertion_point(namespace_scope)
-
-}  // namespace px
-
-#ifndef SWIG
-namespace google {
-namespace protobuf {
-
-template <>
-inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_CoordinateFrameType>() {
-  return ::px::GLOverlay_CoordinateFrameType_descriptor();
-}
-template <>
-inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Mode>() {
-  return ::px::GLOverlay_Mode_descriptor();
-}
-template <>
-inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Identifier>() {
-  return ::px::GLOverlay_Identifier_descriptor();
-}
-
-}  // namespace google
-}  // namespace protobuf
-#endif  // SWIG
-
-// @@protoc_insertion_point(global_scope)
-
-#endif  // PROTOBUF_pixhawk_2eproto__INCLUDED
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/protocol.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/protocol.h
deleted file mode 100644
index 7b3e3c0bd..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/protocol.h
+++ /dev/null
@@ -1,322 +0,0 @@
-#ifndef  _MAVLINK_PROTOCOL_H_
-#define  _MAVLINK_PROTOCOL_H_
-
-#include "string.h"
-#include "mavlink_types.h"
-
-/* 
-   If you want MAVLink on a system that is native big-endian,
-   you need to define NATIVE_BIG_ENDIAN
-*/
-#ifdef NATIVE_BIG_ENDIAN
-# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
-#else
-# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
-#endif
-
-#ifndef MAVLINK_STACK_BUFFER
-#define MAVLINK_STACK_BUFFER 0
-#endif
-
-#ifndef MAVLINK_AVOID_GCC_STACK_BUG
-# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
-#endif
-
-#ifndef MAVLINK_ASSERT
-#define MAVLINK_ASSERT(x)
-#endif
-
-#ifndef MAVLINK_START_UART_SEND
-#define MAVLINK_START_UART_SEND(chan, length)
-#endif
-
-#ifndef MAVLINK_END_UART_SEND
-#define MAVLINK_END_UART_SEND(chan, length)
-#endif
-
-#ifdef MAVLINK_SEPARATE_HELPERS
-#define MAVLINK_HELPER
-#else
-#define MAVLINK_HELPER static inline
-#include "mavlink_helpers.h"
-#endif // MAVLINK_SEPARATE_HELPERS
-
-/* always include the prototypes to ensure we don't get out of sync */
-MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length, uint8_t crc_extra);
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length, uint8_t crc_extra);
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
-						    uint8_t length, uint8_t crc_extra);
-#endif
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t length);
-MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t length);
-MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
-#endif // MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
-MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
-MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
-MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
-MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, 
-					       uint8_t* r_bit_index, uint8_t* buffer);
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
-#endif
-
-/**
- * @brief Get the required buffer size for this message
- */
-static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
-{
-	return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-static inline void byte_swap_2(char *dst, const char *src)
-{
-	dst[0] = src[1];
-	dst[1] = src[0];
-}
-static inline void byte_swap_4(char *dst, const char *src)
-{
-	dst[0] = src[3];
-	dst[1] = src[2];
-	dst[2] = src[1];
-	dst[3] = src[0];
-}
-static inline void byte_swap_8(char *dst, const char *src)
-{
-	dst[0] = src[7];
-	dst[1] = src[6];
-	dst[2] = src[5];
-	dst[3] = src[4];
-	dst[4] = src[3];
-	dst[5] = src[2];
-	dst[6] = src[1];
-	dst[7] = src[0];
-}
-#elif !MAVLINK_ALIGNED_FIELDS
-static inline void byte_copy_2(char *dst, const char *src)
-{
-	dst[0] = src[0];
-	dst[1] = src[1];
-}
-static inline void byte_copy_4(char *dst, const char *src)
-{
-	dst[0] = src[0];
-	dst[1] = src[1];
-	dst[2] = src[2];
-	dst[3] = src[3];
-}
-static inline void byte_copy_8(char *dst, const char *src)
-{
-	memcpy(dst, src, 8);
-}
-#endif
-
-#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
-#define _mav_put_int8_t(buf, wire_offset, b)  buf[wire_offset] = (int8_t)b
-#define _mav_put_char(buf, wire_offset, b)    buf[wire_offset] = b
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int16_t(buf, wire_offset, b)  byte_swap_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int32_t(buf, wire_offset, b)  byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int64_t(buf, wire_offset, b)  byte_swap_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_float(buf, wire_offset, b)    byte_swap_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_double(buf, wire_offset, b)   byte_swap_8(&buf[wire_offset], (const char *)&b)
-#elif !MAVLINK_ALIGNED_FIELDS
-#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int16_t(buf, wire_offset, b)  byte_copy_2(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int32_t(buf, wire_offset, b)  byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_int64_t(buf, wire_offset, b)  byte_copy_8(&buf[wire_offset], (const char *)&b)
-#define _mav_put_float(buf, wire_offset, b)    byte_copy_4(&buf[wire_offset], (const char *)&b)
-#define _mav_put_double(buf, wire_offset, b)   byte_copy_8(&buf[wire_offset], (const char *)&b)
-#else
-#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
-#define _mav_put_int16_t(buf, wire_offset, b)  *(int16_t *)&buf[wire_offset] = b
-#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
-#define _mav_put_int32_t(buf, wire_offset, b)  *(int32_t *)&buf[wire_offset] = b
-#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
-#define _mav_put_int64_t(buf, wire_offset, b)  *(int64_t *)&buf[wire_offset] = b
-#define _mav_put_float(buf, wire_offset, b)    *(float *)&buf[wire_offset] = b
-#define _mav_put_double(buf, wire_offset, b)   *(double *)&buf[wire_offset] = b
-#endif
-
-/*
-  like memcpy(), but if src is NULL, do a memset to zero
-*/
-static void mav_array_memcpy(void *dest, const void *src, size_t n)
-{
-	if (src == NULL) {
-		memset(dest, 0, n);
-	} else {
-		memcpy(dest, src, n);
-	}
-}
-
-/*
- * Place a char array into a buffer
- */
-static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-
-}
-
-/*
- * Place a uint8_t array into a buffer
- */
-static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-
-}
-
-/*
- * Place a int8_t array into a buffer
- */
-static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
-{
-	mav_array_memcpy(&buf[wire_offset], b, array_length);
-
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_PUT_ARRAY(TYPE, V) \
-static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
-{ \
-	if (b == NULL) { \
-		memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
-	} else { \
-		uint16_t i; \
-		for (i=0; i<array_length; i++) { \
-			_mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
-		} \
-	} \
-}
-#else
-#define _MAV_PUT_ARRAY(TYPE, V)					\
-static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
-{ \
-	mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
-}
-#endif
-
-_MAV_PUT_ARRAY(uint16_t, u16)
-_MAV_PUT_ARRAY(uint32_t, u32)
-_MAV_PUT_ARRAY(uint64_t, u64)
-_MAV_PUT_ARRAY(int16_t,  i16)
-_MAV_PUT_ARRAY(int32_t,  i32)
-_MAV_PUT_ARRAY(int64_t,  i64)
-_MAV_PUT_ARRAY(float,    f)
-_MAV_PUT_ARRAY(double,   d)
-
-#define _MAV_RETURN_char(msg, wire_offset)             (const char)_MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_int8_t(msg, wire_offset)   (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
-
-_MAV_MSG_RETURN_TYPE(uint16_t, 2)
-_MAV_MSG_RETURN_TYPE(int16_t,  2)
-_MAV_MSG_RETURN_TYPE(uint32_t, 4)
-_MAV_MSG_RETURN_TYPE(int32_t,  4)
-_MAV_MSG_RETURN_TYPE(uint64_t, 8)
-_MAV_MSG_RETURN_TYPE(int64_t,  8)
-_MAV_MSG_RETURN_TYPE(float,    4)
-_MAV_MSG_RETURN_TYPE(double,   8)
-
-#elif !MAVLINK_ALIGNED_FIELDS
-#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
-
-_MAV_MSG_RETURN_TYPE(uint16_t, 2)
-_MAV_MSG_RETURN_TYPE(int16_t,  2)
-_MAV_MSG_RETURN_TYPE(uint32_t, 4)
-_MAV_MSG_RETURN_TYPE(int32_t,  4)
-_MAV_MSG_RETURN_TYPE(uint64_t, 8)
-_MAV_MSG_RETURN_TYPE(int64_t,  8)
-_MAV_MSG_RETURN_TYPE(float,    4)
-_MAV_MSG_RETURN_TYPE(double,   8)
-#else // nicely aligned, no swap
-#define _MAV_MSG_RETURN_TYPE(TYPE) \
-static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
-{ return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
-
-_MAV_MSG_RETURN_TYPE(uint16_t)
-_MAV_MSG_RETURN_TYPE(int16_t)
-_MAV_MSG_RETURN_TYPE(uint32_t)
-_MAV_MSG_RETURN_TYPE(int32_t)
-_MAV_MSG_RETURN_TYPE(uint64_t)
-_MAV_MSG_RETURN_TYPE(int64_t)
-_MAV_MSG_RETURN_TYPE(float)
-_MAV_MSG_RETURN_TYPE(double)
-#endif // MAVLINK_NEED_BYTE_SWAP
-
-static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, 
-						     uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, 
-							uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, 
-						       uint8_t array_length, uint8_t wire_offset)
-{
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
-	return array_length;
-}
-
-#if MAVLINK_NEED_BYTE_SWAP
-#define _MAV_RETURN_ARRAY(TYPE, V) \
-static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
-							 uint8_t array_length, uint8_t wire_offset) \
-{ \
-	uint16_t i; \
-	for (i=0; i<array_length; i++) { \
-		value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
-	} \
-	return array_length*sizeof(value[0]); \
-}
-#else
-#define _MAV_RETURN_ARRAY(TYPE, V)					\
-static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
-							 uint8_t array_length, uint8_t wire_offset) \
-{ \
-	memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
-	return array_length*sizeof(TYPE); \
-}
-#endif
-
-_MAV_RETURN_ARRAY(uint16_t, u16)
-_MAV_RETURN_ARRAY(uint32_t, u32)
-_MAV_RETURN_ARRAY(uint64_t, u64)
-_MAV_RETURN_ARRAY(int16_t,  i16)
-_MAV_RETURN_ARRAY(int32_t,  i32)
-_MAV_RETURN_ARRAY(int64_t,  i64)
-_MAV_RETURN_ARRAY(float,    f)
-_MAV_RETURN_ARRAY(double,   d)
-
-#endif // _MAVLINK_PROTOCOL_H_
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink.h
deleted file mode 100644
index e596b8fba..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from test.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "test.h"
-
-#endif // MAVLINK_H
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink_msg_test_types.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink_msg_test_types.h
deleted file mode 100644
index 2a3a89ff9..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink_msg_test_types.h
+++ /dev/null
@@ -1,610 +0,0 @@
-// MESSAGE TEST_TYPES PACKING
-
-#define MAVLINK_MSG_ID_TEST_TYPES 0
-
-typedef struct __mavlink_test_types_t
-{
- uint64_t u64; ///< uint64_t
- int64_t s64; ///< int64_t
- double d; ///< double
- uint64_t u64_array[3]; ///< uint64_t_array
- int64_t s64_array[3]; ///< int64_t_array
- double d_array[3]; ///< double_array
- uint32_t u32; ///< uint32_t
- int32_t s32; ///< int32_t
- float f; ///< float
- uint32_t u32_array[3]; ///< uint32_t_array
- int32_t s32_array[3]; ///< int32_t_array
- float f_array[3]; ///< float_array
- uint16_t u16; ///< uint16_t
- int16_t s16; ///< int16_t
- uint16_t u16_array[3]; ///< uint16_t_array
- int16_t s16_array[3]; ///< int16_t_array
- char c; ///< char
- char s[10]; ///< string
- uint8_t u8; ///< uint8_t
- int8_t s8; ///< int8_t
- uint8_t u8_array[3]; ///< uint8_t_array
- int8_t s8_array[3]; ///< int8_t_array
-} mavlink_test_types_t;
-
-#define MAVLINK_MSG_ID_TEST_TYPES_LEN 179
-#define MAVLINK_MSG_ID_0_LEN 179
-
-#define MAVLINK_MSG_TEST_TYPES_FIELD_U64_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S64_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_D_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_U32_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S32_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_F_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_U16_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S16_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S_LEN 10
-#define MAVLINK_MSG_TEST_TYPES_FIELD_U8_ARRAY_LEN 3
-#define MAVLINK_MSG_TEST_TYPES_FIELD_S8_ARRAY_LEN 3
-
-#define MAVLINK_MESSAGE_INFO_TEST_TYPES { \
-	"TEST_TYPES", \
-	22, \
-	{  { "u64", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_test_types_t, u64) }, \
-         { "s64", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_test_types_t, s64) }, \
-         { "d", NULL, MAVLINK_TYPE_DOUBLE, 0, 16, offsetof(mavlink_test_types_t, d) }, \
-         { "u64_array", NULL, MAVLINK_TYPE_UINT64_T, 3, 24, offsetof(mavlink_test_types_t, u64_array) }, \
-         { "s64_array", NULL, MAVLINK_TYPE_INT64_T, 3, 48, offsetof(mavlink_test_types_t, s64_array) }, \
-         { "d_array", NULL, MAVLINK_TYPE_DOUBLE, 3, 72, offsetof(mavlink_test_types_t, d_array) }, \
-         { "u32", "0x%08x", MAVLINK_TYPE_UINT32_T, 0, 96, offsetof(mavlink_test_types_t, u32) }, \
-         { "s32", NULL, MAVLINK_TYPE_INT32_T, 0, 100, offsetof(mavlink_test_types_t, s32) }, \
-         { "f", NULL, MAVLINK_TYPE_FLOAT, 0, 104, offsetof(mavlink_test_types_t, f) }, \
-         { "u32_array", NULL, MAVLINK_TYPE_UINT32_T, 3, 108, offsetof(mavlink_test_types_t, u32_array) }, \
-         { "s32_array", NULL, MAVLINK_TYPE_INT32_T, 3, 120, offsetof(mavlink_test_types_t, s32_array) }, \
-         { "f_array", NULL, MAVLINK_TYPE_FLOAT, 3, 132, offsetof(mavlink_test_types_t, f_array) }, \
-         { "u16", NULL, MAVLINK_TYPE_UINT16_T, 0, 144, offsetof(mavlink_test_types_t, u16) }, \
-         { "s16", NULL, MAVLINK_TYPE_INT16_T, 0, 146, offsetof(mavlink_test_types_t, s16) }, \
-         { "u16_array", NULL, MAVLINK_TYPE_UINT16_T, 3, 148, offsetof(mavlink_test_types_t, u16_array) }, \
-         { "s16_array", NULL, MAVLINK_TYPE_INT16_T, 3, 154, offsetof(mavlink_test_types_t, s16_array) }, \
-         { "c", NULL, MAVLINK_TYPE_CHAR, 0, 160, offsetof(mavlink_test_types_t, c) }, \
-         { "s", NULL, MAVLINK_TYPE_CHAR, 10, 161, offsetof(mavlink_test_types_t, s) }, \
-         { "u8", NULL, MAVLINK_TYPE_UINT8_T, 0, 171, offsetof(mavlink_test_types_t, u8) }, \
-         { "s8", NULL, MAVLINK_TYPE_INT8_T, 0, 172, offsetof(mavlink_test_types_t, s8) }, \
-         { "u8_array", NULL, MAVLINK_TYPE_UINT8_T, 3, 173, offsetof(mavlink_test_types_t, u8_array) }, \
-         { "s8_array", NULL, MAVLINK_TYPE_INT8_T, 3, 176, offsetof(mavlink_test_types_t, s8_array) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a test_types 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 c char
- * @param s string
- * @param u8 uint8_t
- * @param u16 uint16_t
- * @param u32 uint32_t
- * @param u64 uint64_t
- * @param s8 int8_t
- * @param s16 int16_t
- * @param s32 int32_t
- * @param s64 int64_t
- * @param f float
- * @param d double
- * @param u8_array uint8_t_array
- * @param u16_array uint16_t_array
- * @param u32_array uint32_t_array
- * @param u64_array uint64_t_array
- * @param s8_array int8_t_array
- * @param s16_array int16_t_array
- * @param s32_array int32_t_array
- * @param s64_array int64_t_array
- * @param f_array float_array
- * @param d_array double_array
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_test_types_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[179];
-	_mav_put_uint64_t(buf, 0, u64);
-	_mav_put_int64_t(buf, 8, s64);
-	_mav_put_double(buf, 16, d);
-	_mav_put_uint32_t(buf, 96, u32);
-	_mav_put_int32_t(buf, 100, s32);
-	_mav_put_float(buf, 104, f);
-	_mav_put_uint16_t(buf, 144, u16);
-	_mav_put_int16_t(buf, 146, s16);
-	_mav_put_char(buf, 160, c);
-	_mav_put_uint8_t(buf, 171, u8);
-	_mav_put_int8_t(buf, 172, s8);
-	_mav_put_uint64_t_array(buf, 24, u64_array, 3);
-	_mav_put_int64_t_array(buf, 48, s64_array, 3);
-	_mav_put_double_array(buf, 72, d_array, 3);
-	_mav_put_uint32_t_array(buf, 108, u32_array, 3);
-	_mav_put_int32_t_array(buf, 120, s32_array, 3);
-	_mav_put_float_array(buf, 132, f_array, 3);
-	_mav_put_uint16_t_array(buf, 148, u16_array, 3);
-	_mav_put_int16_t_array(buf, 154, s16_array, 3);
-	_mav_put_char_array(buf, 161, s, 10);
-	_mav_put_uint8_t_array(buf, 173, u8_array, 3);
-	_mav_put_int8_t_array(buf, 176, s8_array, 3);
-        memcpy(_MAV_PAYLOAD(msg), buf, 179);
-#else
-	mavlink_test_types_t packet;
-	packet.u64 = u64;
-	packet.s64 = s64;
-	packet.d = d;
-	packet.u32 = u32;
-	packet.s32 = s32;
-	packet.f = f;
-	packet.u16 = u16;
-	packet.s16 = s16;
-	packet.c = c;
-	packet.u8 = u8;
-	packet.s8 = s8;
-	mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
-	mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
-	mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
-	mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
-	mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
-	mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
-	mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
-	mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
-	mav_array_memcpy(packet.s, s, sizeof(char)*10);
-	mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
-	mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
-        memcpy(_MAV_PAYLOAD(msg), &packet, 179);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_TEST_TYPES;
-	return mavlink_finalize_message(msg, system_id, component_id, 179, 103);
-}
-
-/**
- * @brief Pack a test_types 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 was sent over
- * @param msg The MAVLink message to compress the data into
- * @param c char
- * @param s string
- * @param u8 uint8_t
- * @param u16 uint16_t
- * @param u32 uint32_t
- * @param u64 uint64_t
- * @param s8 int8_t
- * @param s16 int16_t
- * @param s32 int32_t
- * @param s64 int64_t
- * @param f float
- * @param d double
- * @param u8_array uint8_t_array
- * @param u16_array uint16_t_array
- * @param u32_array uint32_t_array
- * @param u64_array uint64_t_array
- * @param s8_array int8_t_array
- * @param s16_array int16_t_array
- * @param s32_array int32_t_array
- * @param s64_array int64_t_array
- * @param f_array float_array
- * @param d_array double_array
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[179];
-	_mav_put_uint64_t(buf, 0, u64);
-	_mav_put_int64_t(buf, 8, s64);
-	_mav_put_double(buf, 16, d);
-	_mav_put_uint32_t(buf, 96, u32);
-	_mav_put_int32_t(buf, 100, s32);
-	_mav_put_float(buf, 104, f);
-	_mav_put_uint16_t(buf, 144, u16);
-	_mav_put_int16_t(buf, 146, s16);
-	_mav_put_char(buf, 160, c);
-	_mav_put_uint8_t(buf, 171, u8);
-	_mav_put_int8_t(buf, 172, s8);
-	_mav_put_uint64_t_array(buf, 24, u64_array, 3);
-	_mav_put_int64_t_array(buf, 48, s64_array, 3);
-	_mav_put_double_array(buf, 72, d_array, 3);
-	_mav_put_uint32_t_array(buf, 108, u32_array, 3);
-	_mav_put_int32_t_array(buf, 120, s32_array, 3);
-	_mav_put_float_array(buf, 132, f_array, 3);
-	_mav_put_uint16_t_array(buf, 148, u16_array, 3);
-	_mav_put_int16_t_array(buf, 154, s16_array, 3);
-	_mav_put_char_array(buf, 161, s, 10);
-	_mav_put_uint8_t_array(buf, 173, u8_array, 3);
-	_mav_put_int8_t_array(buf, 176, s8_array, 3);
-        memcpy(_MAV_PAYLOAD(msg), buf, 179);
-#else
-	mavlink_test_types_t packet;
-	packet.u64 = u64;
-	packet.s64 = s64;
-	packet.d = d;
-	packet.u32 = u32;
-	packet.s32 = s32;
-	packet.f = f;
-	packet.u16 = u16;
-	packet.s16 = s16;
-	packet.c = c;
-	packet.u8 = u8;
-	packet.s8 = s8;
-	mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
-	mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
-	mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
-	mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
-	mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
-	mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
-	mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
-	mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
-	mav_array_memcpy(packet.s, s, sizeof(char)*10);
-	mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
-	mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
-        memcpy(_MAV_PAYLOAD(msg), &packet, 179);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_TEST_TYPES;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179, 103);
-}
-
-/**
- * @brief Encode a test_types struct into a 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 test_types C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types)
-{
-	return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array);
-}
-
-/**
- * @brief Send a test_types message
- * @param chan MAVLink channel to send the message
- *
- * @param c char
- * @param s string
- * @param u8 uint8_t
- * @param u16 uint16_t
- * @param u32 uint32_t
- * @param u64 uint64_t
- * @param s8 int8_t
- * @param s16 int16_t
- * @param s32 int32_t
- * @param s64 int64_t
- * @param f float
- * @param d double
- * @param u8_array uint8_t_array
- * @param u16_array uint16_t_array
- * @param u32_array uint32_t_array
- * @param u64_array uint64_t_array
- * @param s8_array int8_t_array
- * @param s16_array int16_t_array
- * @param s32_array int32_t_array
- * @param s64_array int64_t_array
- * @param f_array float_array
- * @param d_array double_array
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[179];
-	_mav_put_uint64_t(buf, 0, u64);
-	_mav_put_int64_t(buf, 8, s64);
-	_mav_put_double(buf, 16, d);
-	_mav_put_uint32_t(buf, 96, u32);
-	_mav_put_int32_t(buf, 100, s32);
-	_mav_put_float(buf, 104, f);
-	_mav_put_uint16_t(buf, 144, u16);
-	_mav_put_int16_t(buf, 146, s16);
-	_mav_put_char(buf, 160, c);
-	_mav_put_uint8_t(buf, 171, u8);
-	_mav_put_int8_t(buf, 172, s8);
-	_mav_put_uint64_t_array(buf, 24, u64_array, 3);
-	_mav_put_int64_t_array(buf, 48, s64_array, 3);
-	_mav_put_double_array(buf, 72, d_array, 3);
-	_mav_put_uint32_t_array(buf, 108, u32_array, 3);
-	_mav_put_int32_t_array(buf, 120, s32_array, 3);
-	_mav_put_float_array(buf, 132, f_array, 3);
-	_mav_put_uint16_t_array(buf, 148, u16_array, 3);
-	_mav_put_int16_t_array(buf, 154, s16_array, 3);
-	_mav_put_char_array(buf, 161, s, 10);
-	_mav_put_uint8_t_array(buf, 173, u8_array, 3);
-	_mav_put_int8_t_array(buf, 176, s8_array, 3);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, buf, 179, 103);
-#else
-	mavlink_test_types_t packet;
-	packet.u64 = u64;
-	packet.s64 = s64;
-	packet.d = d;
-	packet.u32 = u32;
-	packet.s32 = s32;
-	packet.f = f;
-	packet.u16 = u16;
-	packet.s16 = s16;
-	packet.c = c;
-	packet.u8 = u8;
-	packet.s8 = s8;
-	mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
-	mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
-	mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
-	mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
-	mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
-	mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
-	mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
-	mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
-	mav_array_memcpy(packet.s, s, sizeof(char)*10);
-	mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
-	mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, (const char *)&packet, 179, 103);
-#endif
-}
-
-#endif
-
-// MESSAGE TEST_TYPES UNPACKING
-
-
-/**
- * @brief Get field c from test_types message
- *
- * @return char
- */
-static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_char(msg,  160);
-}
-
-/**
- * @brief Get field s from test_types message
- *
- * @return string
- */
-static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s)
-{
-	return _MAV_RETURN_char_array(msg, s, 10,  161);
-}
-
-/**
- * @brief Get field u8 from test_types message
- *
- * @return uint8_t
- */
-static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  171);
-}
-
-/**
- * @brief Get field u16 from test_types message
- *
- * @return uint16_t
- */
-static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  144);
-}
-
-/**
- * @brief Get field u32 from test_types message
- *
- * @return uint32_t
- */
-static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  96);
-}
-
-/**
- * @brief Get field u64 from test_types message
- *
- * @return uint64_t
- */
-static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint64_t(msg,  0);
-}
-
-/**
- * @brief Get field s8 from test_types message
- *
- * @return int8_t
- */
-static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int8_t(msg,  172);
-}
-
-/**
- * @brief Get field s16 from test_types message
- *
- * @return int16_t
- */
-static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  146);
-}
-
-/**
- * @brief Get field s32 from test_types message
- *
- * @return int32_t
- */
-static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  100);
-}
-
-/**
- * @brief Get field s64 from test_types message
- *
- * @return int64_t
- */
-static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int64_t(msg,  8);
-}
-
-/**
- * @brief Get field f from test_types message
- *
- * @return float
- */
-static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  104);
-}
-
-/**
- * @brief Get field d from test_types message
- *
- * @return double
- */
-static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_double(msg,  16);
-}
-
-/**
- * @brief Get field u8_array from test_types message
- *
- * @return uint8_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array)
-{
-	return _MAV_RETURN_uint8_t_array(msg, u8_array, 3,  173);
-}
-
-/**
- * @brief Get field u16_array from test_types message
- *
- * @return uint16_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array)
-{
-	return _MAV_RETURN_uint16_t_array(msg, u16_array, 3,  148);
-}
-
-/**
- * @brief Get field u32_array from test_types message
- *
- * @return uint32_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array)
-{
-	return _MAV_RETURN_uint32_t_array(msg, u32_array, 3,  108);
-}
-
-/**
- * @brief Get field u64_array from test_types message
- *
- * @return uint64_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array)
-{
-	return _MAV_RETURN_uint64_t_array(msg, u64_array, 3,  24);
-}
-
-/**
- * @brief Get field s8_array from test_types message
- *
- * @return int8_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array)
-{
-	return _MAV_RETURN_int8_t_array(msg, s8_array, 3,  176);
-}
-
-/**
- * @brief Get field s16_array from test_types message
- *
- * @return int16_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array)
-{
-	return _MAV_RETURN_int16_t_array(msg, s16_array, 3,  154);
-}
-
-/**
- * @brief Get field s32_array from test_types message
- *
- * @return int32_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array)
-{
-	return _MAV_RETURN_int32_t_array(msg, s32_array, 3,  120);
-}
-
-/**
- * @brief Get field s64_array from test_types message
- *
- * @return int64_t_array
- */
-static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array)
-{
-	return _MAV_RETURN_int64_t_array(msg, s64_array, 3,  48);
-}
-
-/**
- * @brief Get field f_array from test_types message
- *
- * @return float_array
- */
-static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array)
-{
-	return _MAV_RETURN_float_array(msg, f_array, 3,  132);
-}
-
-/**
- * @brief Get field d_array from test_types message
- *
- * @return double_array
- */
-static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array)
-{
-	return _MAV_RETURN_double_array(msg, d_array, 3,  72);
-}
-
-/**
- * @brief Decode a test_types message into a struct
- *
- * @param msg The message to decode
- * @param test_types C-struct to decode the message contents into
- */
-static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, mavlink_test_types_t* test_types)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	test_types->u64 = mavlink_msg_test_types_get_u64(msg);
-	test_types->s64 = mavlink_msg_test_types_get_s64(msg);
-	test_types->d = mavlink_msg_test_types_get_d(msg);
-	mavlink_msg_test_types_get_u64_array(msg, test_types->u64_array);
-	mavlink_msg_test_types_get_s64_array(msg, test_types->s64_array);
-	mavlink_msg_test_types_get_d_array(msg, test_types->d_array);
-	test_types->u32 = mavlink_msg_test_types_get_u32(msg);
-	test_types->s32 = mavlink_msg_test_types_get_s32(msg);
-	test_types->f = mavlink_msg_test_types_get_f(msg);
-	mavlink_msg_test_types_get_u32_array(msg, test_types->u32_array);
-	mavlink_msg_test_types_get_s32_array(msg, test_types->s32_array);
-	mavlink_msg_test_types_get_f_array(msg, test_types->f_array);
-	test_types->u16 = mavlink_msg_test_types_get_u16(msg);
-	test_types->s16 = mavlink_msg_test_types_get_s16(msg);
-	mavlink_msg_test_types_get_u16_array(msg, test_types->u16_array);
-	mavlink_msg_test_types_get_s16_array(msg, test_types->s16_array);
-	test_types->c = mavlink_msg_test_types_get_c(msg);
-	mavlink_msg_test_types_get_s(msg, test_types->s);
-	test_types->u8 = mavlink_msg_test_types_get_u8(msg);
-	test_types->s8 = mavlink_msg_test_types_get_s8(msg);
-	mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array);
-	mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array);
-#else
-	memcpy(test_types, _MAV_PAYLOAD(msg), 179);
-#endif
-}
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h
deleted file mode 100644
index 4dc04f889..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol generated from test.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef TEST_H
-#define TEST_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// MESSAGE LENGTHS AND CRCS
-
-#ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
-#endif
-
-#ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {103, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
-#endif
-
-#ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_TEST
-
-
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-// ENUM DEFINITIONS
-
-
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_test_types.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // TEST_H
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h
deleted file mode 100644
index 658e1ae07..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from test.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef TEST_TESTSUITE_H
-#define TEST_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-
-static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-
-	mavlink_test_test(system_id, component_id, last_msg);
-}
-#endif
-
-
-
-
-static void mavlink_test_test_types(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_test_types_t packet_in = {
-		93372036854775807ULL,
-	93372036854776311LL,
-	235.0,
-	{ 93372036854777319, 93372036854777320, 93372036854777321 },
-	{ 93372036854778831, 93372036854778832, 93372036854778833 },
-	{ 627.0, 628.0, 629.0 },
-	963502456,
-	963502664,
-	745.0,
-	{ 963503080, 963503081, 963503082 },
-	{ 963503704, 963503705, 963503706 },
-	{ 941.0, 942.0, 943.0 },
-	24723,
-	24827,
-	{ 24931, 24932, 24933 },
-	{ 25243, 25244, 25245 },
-	'E',
-	"FGHIJKLMN",
-	198,
-	9,
-	{ 76, 77, 78 },
-	{ 21, 22, 23 },
-	};
-	mavlink_test_types_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.u64 = packet_in.u64;
-        	packet1.s64 = packet_in.s64;
-        	packet1.d = packet_in.d;
-        	packet1.u32 = packet_in.u32;
-        	packet1.s32 = packet_in.s32;
-        	packet1.f = packet_in.f;
-        	packet1.u16 = packet_in.u16;
-        	packet1.s16 = packet_in.s16;
-        	packet1.c = packet_in.c;
-        	packet1.u8 = packet_in.u8;
-        	packet1.s8 = packet_in.s8;
-        
-        	mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3);
-        	mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3);
-        	mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3);
-        	mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3);
-        	mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3);
-        	mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3);
-        	mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3);
-        	mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3);
-        	mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10);
-        	mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3);
-        	mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_test_types_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
-	mavlink_msg_test_types_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
-	mavlink_msg_test_types_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_test_types_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_test_types_send(MAVLINK_COMM_1 , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
-	mavlink_msg_test_types_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_test(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_test_types(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // TEST_TESTSUITE_H
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/version.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/version.h
deleted file mode 100644
index 867641e21..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/version.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/** @file
- *	@brief MAVLink comm protocol built from test.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "Thu Mar  1 15:11:58 2012"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179
- 
-#endif // MAVLINK_VERSION_H
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/src_v1.0/pixhawk/pixhawk.pb.cc b/mavlink/share/pyshared/pymavlink/generator/C/src_v1.0/pixhawk/pixhawk.pb.cc
deleted file mode 100644
index e984f512a..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/src_v1.0/pixhawk/pixhawk.pb.cc
+++ /dev/null
@@ -1,5431 +0,0 @@
-// Generated by the protocol buffer compiler.  DO NOT EDIT!
-
-#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
-#include "pixhawk.pb.h"
-
-#include <algorithm>
-
-#include <google/protobuf/stubs/once.h>
-#include <google/protobuf/io/coded_stream.h>
-#include <google/protobuf/wire_format_lite_inl.h>
-#include <google/protobuf/descriptor.h>
-#include <google/protobuf/reflection_ops.h>
-#include <google/protobuf/wire_format.h>
-// @@protoc_insertion_point(includes)
-
-namespace px {
-
-namespace {
-
-const ::google::protobuf::Descriptor* HeaderInfo_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  HeaderInfo_reflection_ = NULL;
-const ::google::protobuf::Descriptor* GLOverlay_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  GLOverlay_reflection_ = NULL;
-const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor_ = NULL;
-const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor_ = NULL;
-const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor_ = NULL;
-const ::google::protobuf::Descriptor* Obstacle_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  Obstacle_reflection_ = NULL;
-const ::google::protobuf::Descriptor* ObstacleList_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  ObstacleList_reflection_ = NULL;
-const ::google::protobuf::Descriptor* ObstacleMap_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  ObstacleMap_reflection_ = NULL;
-const ::google::protobuf::Descriptor* Path_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  Path_reflection_ = NULL;
-const ::google::protobuf::Descriptor* PointCloudXYZI_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  PointCloudXYZI_reflection_ = NULL;
-const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  PointCloudXYZI_PointXYZI_reflection_ = NULL;
-const ::google::protobuf::Descriptor* PointCloudXYZRGB_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  PointCloudXYZRGB_reflection_ = NULL;
-const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  PointCloudXYZRGB_PointXYZRGB_reflection_ = NULL;
-const ::google::protobuf::Descriptor* RGBDImage_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  RGBDImage_reflection_ = NULL;
-const ::google::protobuf::Descriptor* Waypoint_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  Waypoint_reflection_ = NULL;
-
-}  // namespace
-
-
-void protobuf_AssignDesc_pixhawk_2eproto() {
-  protobuf_AddDesc_pixhawk_2eproto();
-  const ::google::protobuf::FileDescriptor* file =
-    ::google::protobuf::DescriptorPool::generated_pool()->FindFileByName(
-      "pixhawk.proto");
-  GOOGLE_CHECK(file != NULL);
-  HeaderInfo_descriptor_ = file->message_type(0);
-  static const int HeaderInfo_offsets_[3] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, source_sysid_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, source_compid_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, timestamp_),
-  };
-  HeaderInfo_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      HeaderInfo_descriptor_,
-      HeaderInfo::default_instance_,
-      HeaderInfo_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(HeaderInfo, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(HeaderInfo));
-  GLOverlay_descriptor_ = file->message_type(1);
-  static const int GLOverlay_offsets_[7] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, name_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, coordinateframetype_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, data_),
-  };
-  GLOverlay_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      GLOverlay_descriptor_,
-      GLOverlay::default_instance_,
-      GLOverlay_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(GLOverlay));
-  GLOverlay_CoordinateFrameType_descriptor_ = GLOverlay_descriptor_->enum_type(0);
-  GLOverlay_Mode_descriptor_ = GLOverlay_descriptor_->enum_type(1);
-  GLOverlay_Identifier_descriptor_ = GLOverlay_descriptor_->enum_type(2);
-  Obstacle_descriptor_ = file->message_type(2);
-  static const int Obstacle_offsets_[6] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, length_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, width_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, height_),
-  };
-  Obstacle_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      Obstacle_descriptor_,
-      Obstacle::default_instance_,
-      Obstacle_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(Obstacle));
-  ObstacleList_descriptor_ = file->message_type(3);
-  static const int ObstacleList_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, obstacles_),
-  };
-  ObstacleList_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      ObstacleList_descriptor_,
-      ObstacleList::default_instance_,
-      ObstacleList_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(ObstacleList));
-  ObstacleMap_descriptor_ = file->message_type(4);
-  static const int ObstacleMap_offsets_[10] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, type_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, resolution_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, rows_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, cols_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapr0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapc0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayr0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayc0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, data_),
-  };
-  ObstacleMap_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      ObstacleMap_descriptor_,
-      ObstacleMap::default_instance_,
-      ObstacleMap_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(ObstacleMap));
-  Path_descriptor_ = file->message_type(5);
-  static const int Path_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, waypoints_),
-  };
-  Path_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      Path_descriptor_,
-      Path::default_instance_,
-      Path_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(Path));
-  PointCloudXYZI_descriptor_ = file->message_type(6);
-  static const int PointCloudXYZI_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, points_),
-  };
-  PointCloudXYZI_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      PointCloudXYZI_descriptor_,
-      PointCloudXYZI::default_instance_,
-      PointCloudXYZI_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(PointCloudXYZI));
-  PointCloudXYZI_PointXYZI_descriptor_ = PointCloudXYZI_descriptor_->nested_type(0);
-  static const int PointCloudXYZI_PointXYZI_offsets_[4] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, intensity_),
-  };
-  PointCloudXYZI_PointXYZI_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      PointCloudXYZI_PointXYZI_descriptor_,
-      PointCloudXYZI_PointXYZI::default_instance_,
-      PointCloudXYZI_PointXYZI_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI_PointXYZI, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(PointCloudXYZI_PointXYZI));
-  PointCloudXYZRGB_descriptor_ = file->message_type(7);
-  static const int PointCloudXYZRGB_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, points_),
-  };
-  PointCloudXYZRGB_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      PointCloudXYZRGB_descriptor_,
-      PointCloudXYZRGB::default_instance_,
-      PointCloudXYZRGB_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(PointCloudXYZRGB));
-  PointCloudXYZRGB_PointXYZRGB_descriptor_ = PointCloudXYZRGB_descriptor_->nested_type(0);
-  static const int PointCloudXYZRGB_PointXYZRGB_offsets_[4] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, rgb_),
-  };
-  PointCloudXYZRGB_PointXYZRGB_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      PointCloudXYZRGB_PointXYZRGB_descriptor_,
-      PointCloudXYZRGB_PointXYZRGB::default_instance_,
-      PointCloudXYZRGB_PointXYZRGB_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB_PointXYZRGB, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(PointCloudXYZRGB_PointXYZRGB));
-  RGBDImage_descriptor_ = file->message_type(8);
-  static const int RGBDImage_offsets_[21] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, cols_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, rows_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, step1_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, type1_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, imagedata1_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, step2_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, type2_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, imagedata2_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_config_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_type_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, roll_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, pitch_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, yaw_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, lon_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, lat_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, alt_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, ground_z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, camera_matrix_),
-  };
-  RGBDImage_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      RGBDImage_descriptor_,
-      RGBDImage::default_instance_,
-      RGBDImage_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(RGBDImage));
-  Waypoint_descriptor_ = file->message_type(9);
-  static const int Waypoint_offsets_[6] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, roll_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, pitch_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, yaw_),
-  };
-  Waypoint_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      Waypoint_descriptor_,
-      Waypoint::default_instance_,
-      Waypoint_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(Waypoint));
-}
-
-namespace {
-
-GOOGLE_PROTOBUF_DECLARE_ONCE(protobuf_AssignDescriptors_once_);
-inline void protobuf_AssignDescriptorsOnce() {
-  ::google::protobuf::GoogleOnceInit(&protobuf_AssignDescriptors_once_,
-                 &protobuf_AssignDesc_pixhawk_2eproto);
-}
-
-void protobuf_RegisterTypes(const ::std::string&) {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    HeaderInfo_descriptor_, &HeaderInfo::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    GLOverlay_descriptor_, &GLOverlay::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    Obstacle_descriptor_, &Obstacle::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    ObstacleList_descriptor_, &ObstacleList::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    ObstacleMap_descriptor_, &ObstacleMap::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    Path_descriptor_, &Path::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    PointCloudXYZI_descriptor_, &PointCloudXYZI::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    PointCloudXYZI_PointXYZI_descriptor_, &PointCloudXYZI_PointXYZI::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    PointCloudXYZRGB_descriptor_, &PointCloudXYZRGB::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    PointCloudXYZRGB_PointXYZRGB_descriptor_, &PointCloudXYZRGB_PointXYZRGB::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    RGBDImage_descriptor_, &RGBDImage::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    Waypoint_descriptor_, &Waypoint::default_instance());
-}
-
-}  // namespace
-
-void protobuf_ShutdownFile_pixhawk_2eproto() {
-  delete HeaderInfo::default_instance_;
-  delete HeaderInfo_reflection_;
-  delete GLOverlay::default_instance_;
-  delete GLOverlay_reflection_;
-  delete Obstacle::default_instance_;
-  delete Obstacle_reflection_;
-  delete ObstacleList::default_instance_;
-  delete ObstacleList_reflection_;
-  delete ObstacleMap::default_instance_;
-  delete ObstacleMap_reflection_;
-  delete Path::default_instance_;
-  delete Path_reflection_;
-  delete PointCloudXYZI::default_instance_;
-  delete PointCloudXYZI_reflection_;
-  delete PointCloudXYZI_PointXYZI::default_instance_;
-  delete PointCloudXYZI_PointXYZI_reflection_;
-  delete PointCloudXYZRGB::default_instance_;
-  delete PointCloudXYZRGB_reflection_;
-  delete PointCloudXYZRGB_PointXYZRGB::default_instance_;
-  delete PointCloudXYZRGB_PointXYZRGB_reflection_;
-  delete RGBDImage::default_instance_;
-  delete RGBDImage_reflection_;
-  delete Waypoint::default_instance_;
-  delete Waypoint_reflection_;
-}
-
-void protobuf_AddDesc_pixhawk_2eproto() {
-  static bool already_here = false;
-  if (already_here) return;
-  already_here = true;
-  GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-  ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-    "\n\rpixhawk.proto\022\002px\"L\n\nHeaderInfo\022\024\n\014sou"
-    "rce_sysid\030\001 \002(\005\022\025\n\rsource_compid\030\002 \002(\005\022\021"
-    "\n\ttimestamp\030\003 \002(\001\"\377\004\n\tGLOverlay\022\036\n\006heade"
-    "r\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004name\030\002 \001(\t\022>\n"
-    "\023coordinateFrameType\030\003 \001(\0162!.px.GLOverla"
-    "y.CoordinateFrameType\022\020\n\010origin_x\030\004 \001(\001\022"
-    "\020\n\010origin_y\030\005 \001(\001\022\020\n\010origin_z\030\006 \001(\001\022\014\n\004d"
-    "ata\030\007 \001(\014\",\n\023CoordinateFrameType\022\n\n\006GLOB"
-    "AL\020\000\022\t\n\005LOCAL\020\001\"\333\001\n\004Mode\022\n\n\006POINTS\020\000\022\t\n\005"
-    "LINES\020\001\022\016\n\nLINE_STRIP\020\002\022\r\n\tLINE_LOOP\020\003\022\r"
-    "\n\tTRIANGLES\020\004\022\022\n\016TRIANGLE_STRIP\020\005\022\020\n\014TRI"
-    "ANGLE_FAN\020\006\022\t\n\005QUADS\020\007\022\016\n\nQUAD_STRIP\020\010\022\013"
-    "\n\007POLYGON\020\t\022\020\n\014SOLID_CIRCLE\020\n\022\017\n\013WIRE_CI"
-    "RCLE\020\013\022\016\n\nSOLID_CUBE\020\014\022\r\n\tWIRE_CUBE\020\r\"\263\001"
-    "\n\nIdentifier\022\007\n\003END\020\016\022\014\n\010VERTEX2F\020\017\022\014\n\010V"
-    "ERTEX3F\020\020\022\013\n\007ROTATEF\020\021\022\016\n\nTRANSLATEF\020\022\022\n"
-    "\n\006SCALEF\020\023\022\017\n\013PUSH_MATRIX\020\024\022\016\n\nPOP_MATRI"
-    "X\020\025\022\013\n\007COLOR3F\020\026\022\013\n\007COLOR4F\020\027\022\r\n\tPOINTSI"
-    "ZE\020\030\022\r\n\tLINEWIDTH\020\031\"Z\n\010Obstacle\022\t\n\001x\030\001 \001"
-    "(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001z\030\003 \001(\002\022\016\n\006length\030\004 \001(\002"
-    "\022\r\n\005width\030\005 \001(\002\022\016\n\006height\030\006 \001(\002\"O\n\014Obsta"
-    "cleList\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo\022"
-    "\037\n\tobstacles\030\002 \003(\0132\014.px.Obstacle\"\271\001\n\013Obs"
-    "tacleMap\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo"
-    "\022\014\n\004type\030\002 \002(\005\022\022\n\nresolution\030\003 \001(\002\022\014\n\004ro"
-    "ws\030\004 \001(\005\022\014\n\004cols\030\005 \001(\005\022\r\n\005mapR0\030\006 \001(\005\022\r\n"
-    "\005mapC0\030\007 \001(\005\022\017\n\007arrayR0\030\010 \001(\005\022\017\n\007arrayC0"
-    "\030\t \001(\005\022\014\n\004data\030\n \001(\014\"G\n\004Path\022\036\n\006header\030\001"
-    " \002(\0132\016.px.HeaderInfo\022\037\n\twaypoints\030\002 \003(\0132"
-    "\014.px.Waypoint\"\237\001\n\016PointCloudXYZI\022\036\n\006head"
-    "er\030\001 \002(\0132\016.px.HeaderInfo\022,\n\006points\030\002 \003(\013"
-    "2\034.px.PointCloudXYZI.PointXYZI\032\?\n\tPointX"
-    "YZI\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z\030\003 \002(\002\022\021\n\t"
-    "intensity\030\004 \002(\002\"\241\001\n\020PointCloudXYZRGB\022\036\n\006"
-    "header\030\001 \002(\0132\016.px.HeaderInfo\0220\n\006points\030\002"
-    " \003(\0132 .px.PointCloudXYZRGB.PointXYZRGB\032;"
-    "\n\013PointXYZRGB\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z"
-    "\030\003 \002(\002\022\013\n\003rgb\030\004 \002(\002\"\365\002\n\tRGBDImage\022\036\n\006hea"
-    "der\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004cols\030\002 \002(\r\022"
-    "\014\n\004rows\030\003 \002(\r\022\r\n\005step1\030\004 \002(\r\022\r\n\005type1\030\005 "
-    "\002(\r\022\022\n\nimageData1\030\006 \002(\014\022\r\n\005step2\030\007 \002(\r\022\r"
-    "\n\005type2\030\010 \002(\r\022\022\n\nimageData2\030\t \002(\014\022\025\n\rcam"
-    "era_config\030\n \001(\r\022\023\n\013camera_type\030\013 \001(\r\022\014\n"
-    "\004roll\030\014 \001(\002\022\r\n\005pitch\030\r \001(\002\022\013\n\003yaw\030\016 \001(\002\022"
-    "\013\n\003lon\030\017 \001(\002\022\013\n\003lat\030\020 \001(\002\022\013\n\003alt\030\021 \001(\002\022\020"
-    "\n\010ground_x\030\022 \001(\002\022\020\n\010ground_y\030\023 \001(\002\022\020\n\010gr"
-    "ound_z\030\024 \001(\002\022\025\n\rcamera_matrix\030\025 \003(\002\"U\n\010W"
-    "aypoint\022\t\n\001x\030\001 \002(\001\022\t\n\001y\030\002 \002(\001\022\t\n\001z\030\003 \001(\001"
-    "\022\014\n\004roll\030\004 \001(\001\022\r\n\005pitch\030\005 \001(\001\022\013\n\003yaw\030\006 \001"
-    "(\001", 1962);
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
-    "pixhawk.proto", &protobuf_RegisterTypes);
-  HeaderInfo::default_instance_ = new HeaderInfo();
-  GLOverlay::default_instance_ = new GLOverlay();
-  Obstacle::default_instance_ = new Obstacle();
-  ObstacleList::default_instance_ = new ObstacleList();
-  ObstacleMap::default_instance_ = new ObstacleMap();
-  Path::default_instance_ = new Path();
-  PointCloudXYZI::default_instance_ = new PointCloudXYZI();
-  PointCloudXYZI_PointXYZI::default_instance_ = new PointCloudXYZI_PointXYZI();
-  PointCloudXYZRGB::default_instance_ = new PointCloudXYZRGB();
-  PointCloudXYZRGB_PointXYZRGB::default_instance_ = new PointCloudXYZRGB_PointXYZRGB();
-  RGBDImage::default_instance_ = new RGBDImage();
-  Waypoint::default_instance_ = new Waypoint();
-  HeaderInfo::default_instance_->InitAsDefaultInstance();
-  GLOverlay::default_instance_->InitAsDefaultInstance();
-  Obstacle::default_instance_->InitAsDefaultInstance();
-  ObstacleList::default_instance_->InitAsDefaultInstance();
-  ObstacleMap::default_instance_->InitAsDefaultInstance();
-  Path::default_instance_->InitAsDefaultInstance();
-  PointCloudXYZI::default_instance_->InitAsDefaultInstance();
-  PointCloudXYZI_PointXYZI::default_instance_->InitAsDefaultInstance();
-  PointCloudXYZRGB::default_instance_->InitAsDefaultInstance();
-  PointCloudXYZRGB_PointXYZRGB::default_instance_->InitAsDefaultInstance();
-  RGBDImage::default_instance_->InitAsDefaultInstance();
-  Waypoint::default_instance_->InitAsDefaultInstance();
-  ::google::protobuf::internal::OnShutdown(&protobuf_ShutdownFile_pixhawk_2eproto);
-}
-
-// Force AddDescriptors() to be called at static initialization time.
-struct StaticDescriptorInitializer_pixhawk_2eproto {
-  StaticDescriptorInitializer_pixhawk_2eproto() {
-    protobuf_AddDesc_pixhawk_2eproto();
-  }
-} static_descriptor_initializer_pixhawk_2eproto_;
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int HeaderInfo::kSourceSysidFieldNumber;
-const int HeaderInfo::kSourceCompidFieldNumber;
-const int HeaderInfo::kTimestampFieldNumber;
-#endif  // !_MSC_VER
-
-HeaderInfo::HeaderInfo()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void HeaderInfo::InitAsDefaultInstance() {
-}
-
-HeaderInfo::HeaderInfo(const HeaderInfo& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void HeaderInfo::SharedCtor() {
-  _cached_size_ = 0;
-  source_sysid_ = 0;
-  source_compid_ = 0;
-  timestamp_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-HeaderInfo::~HeaderInfo() {
-  SharedDtor();
-}
-
-void HeaderInfo::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void HeaderInfo::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* HeaderInfo::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return HeaderInfo_descriptor_;
-}
-
-const HeaderInfo& HeaderInfo::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-HeaderInfo* HeaderInfo::default_instance_ = NULL;
-
-HeaderInfo* HeaderInfo::New() const {
-  return new HeaderInfo;
-}
-
-void HeaderInfo::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    source_sysid_ = 0;
-    source_compid_ = 0;
-    timestamp_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool HeaderInfo::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required int32 source_sysid = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &source_sysid_)));
-          set_has_source_sysid();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(16)) goto parse_source_compid;
-        break;
-      }
-      
-      // required int32 source_compid = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_source_compid:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &source_compid_)));
-          set_has_source_compid();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(25)) goto parse_timestamp;
-        break;
-      }
-      
-      // required double timestamp = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_timestamp:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &timestamp_)));
-          set_has_timestamp();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void HeaderInfo::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required int32 source_sysid = 1;
-  if (has_source_sysid()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(1, this->source_sysid(), output);
-  }
-  
-  // required int32 source_compid = 2;
-  if (has_source_compid()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->source_compid(), output);
-  }
-  
-  // required double timestamp = 3;
-  if (has_timestamp()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->timestamp(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* HeaderInfo::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required int32 source_sysid = 1;
-  if (has_source_sysid()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(1, this->source_sysid(), target);
-  }
-  
-  // required int32 source_compid = 2;
-  if (has_source_compid()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->source_compid(), target);
-  }
-  
-  // required double timestamp = 3;
-  if (has_timestamp()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->timestamp(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int HeaderInfo::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required int32 source_sysid = 1;
-    if (has_source_sysid()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->source_sysid());
-    }
-    
-    // required int32 source_compid = 2;
-    if (has_source_compid()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->source_compid());
-    }
-    
-    // required double timestamp = 3;
-    if (has_timestamp()) {
-      total_size += 1 + 8;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void HeaderInfo::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const HeaderInfo* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const HeaderInfo*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void HeaderInfo::MergeFrom(const HeaderInfo& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_source_sysid()) {
-      set_source_sysid(from.source_sysid());
-    }
-    if (from.has_source_compid()) {
-      set_source_compid(from.source_compid());
-    }
-    if (from.has_timestamp()) {
-      set_timestamp(from.timestamp());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void HeaderInfo::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void HeaderInfo::CopyFrom(const HeaderInfo& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool HeaderInfo::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000007) != 0x00000007) return false;
-  
-  return true;
-}
-
-void HeaderInfo::Swap(HeaderInfo* other) {
-  if (other != this) {
-    std::swap(source_sysid_, other->source_sysid_);
-    std::swap(source_compid_, other->source_compid_);
-    std::swap(timestamp_, other->timestamp_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata HeaderInfo::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = HeaderInfo_descriptor_;
-  metadata.reflection = HeaderInfo_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return GLOverlay_CoordinateFrameType_descriptor_;
-}
-bool GLOverlay_CoordinateFrameType_IsValid(int value) {
-  switch(value) {
-    case 0:
-    case 1:
-      return true;
-    default:
-      return false;
-  }
-}
-
-#ifndef _MSC_VER
-const GLOverlay_CoordinateFrameType GLOverlay::GLOBAL;
-const GLOverlay_CoordinateFrameType GLOverlay::LOCAL;
-const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MIN;
-const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MAX;
-const int GLOverlay::CoordinateFrameType_ARRAYSIZE;
-#endif  // _MSC_VER
-const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return GLOverlay_Mode_descriptor_;
-}
-bool GLOverlay_Mode_IsValid(int value) {
-  switch(value) {
-    case 0:
-    case 1:
-    case 2:
-    case 3:
-    case 4:
-    case 5:
-    case 6:
-    case 7:
-    case 8:
-    case 9:
-    case 10:
-    case 11:
-    case 12:
-    case 13:
-      return true;
-    default:
-      return false;
-  }
-}
-
-#ifndef _MSC_VER
-const GLOverlay_Mode GLOverlay::POINTS;
-const GLOverlay_Mode GLOverlay::LINES;
-const GLOverlay_Mode GLOverlay::LINE_STRIP;
-const GLOverlay_Mode GLOverlay::LINE_LOOP;
-const GLOverlay_Mode GLOverlay::TRIANGLES;
-const GLOverlay_Mode GLOverlay::TRIANGLE_STRIP;
-const GLOverlay_Mode GLOverlay::TRIANGLE_FAN;
-const GLOverlay_Mode GLOverlay::QUADS;
-const GLOverlay_Mode GLOverlay::QUAD_STRIP;
-const GLOverlay_Mode GLOverlay::POLYGON;
-const GLOverlay_Mode GLOverlay::SOLID_CIRCLE;
-const GLOverlay_Mode GLOverlay::WIRE_CIRCLE;
-const GLOverlay_Mode GLOverlay::SOLID_CUBE;
-const GLOverlay_Mode GLOverlay::WIRE_CUBE;
-const GLOverlay_Mode GLOverlay::Mode_MIN;
-const GLOverlay_Mode GLOverlay::Mode_MAX;
-const int GLOverlay::Mode_ARRAYSIZE;
-#endif  // _MSC_VER
-const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return GLOverlay_Identifier_descriptor_;
-}
-bool GLOverlay_Identifier_IsValid(int value) {
-  switch(value) {
-    case 14:
-    case 15:
-    case 16:
-    case 17:
-    case 18:
-    case 19:
-    case 20:
-    case 21:
-    case 22:
-    case 23:
-    case 24:
-    case 25:
-      return true;
-    default:
-      return false;
-  }
-}
-
-#ifndef _MSC_VER
-const GLOverlay_Identifier GLOverlay::END;
-const GLOverlay_Identifier GLOverlay::VERTEX2F;
-const GLOverlay_Identifier GLOverlay::VERTEX3F;
-const GLOverlay_Identifier GLOverlay::ROTATEF;
-const GLOverlay_Identifier GLOverlay::TRANSLATEF;
-const GLOverlay_Identifier GLOverlay::SCALEF;
-const GLOverlay_Identifier GLOverlay::PUSH_MATRIX;
-const GLOverlay_Identifier GLOverlay::POP_MATRIX;
-const GLOverlay_Identifier GLOverlay::COLOR3F;
-const GLOverlay_Identifier GLOverlay::COLOR4F;
-const GLOverlay_Identifier GLOverlay::POINTSIZE;
-const GLOverlay_Identifier GLOverlay::LINEWIDTH;
-const GLOverlay_Identifier GLOverlay::Identifier_MIN;
-const GLOverlay_Identifier GLOverlay::Identifier_MAX;
-const int GLOverlay::Identifier_ARRAYSIZE;
-#endif  // _MSC_VER
-#ifndef _MSC_VER
-const int GLOverlay::kHeaderFieldNumber;
-const int GLOverlay::kNameFieldNumber;
-const int GLOverlay::kCoordinateFrameTypeFieldNumber;
-const int GLOverlay::kOriginXFieldNumber;
-const int GLOverlay::kOriginYFieldNumber;
-const int GLOverlay::kOriginZFieldNumber;
-const int GLOverlay::kDataFieldNumber;
-#endif  // !_MSC_VER
-
-GLOverlay::GLOverlay()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void GLOverlay::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-GLOverlay::GLOverlay(const GLOverlay& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void GLOverlay::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  coordinateframetype_ = 0;
-  origin_x_ = 0;
-  origin_y_ = 0;
-  origin_z_ = 0;
-  data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-GLOverlay::~GLOverlay() {
-  SharedDtor();
-}
-
-void GLOverlay::SharedDtor() {
-  if (name_ != &::google::protobuf::internal::kEmptyString) {
-    delete name_;
-  }
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    delete data_;
-  }
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void GLOverlay::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* GLOverlay::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return GLOverlay_descriptor_;
-}
-
-const GLOverlay& GLOverlay::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-GLOverlay* GLOverlay::default_instance_ = NULL;
-
-GLOverlay* GLOverlay::New() const {
-  return new GLOverlay;
-}
-
-void GLOverlay::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-    if (has_name()) {
-      if (name_ != &::google::protobuf::internal::kEmptyString) {
-        name_->clear();
-      }
-    }
-    coordinateframetype_ = 0;
-    origin_x_ = 0;
-    origin_y_ = 0;
-    origin_z_ = 0;
-    if (has_data()) {
-      if (data_ != &::google::protobuf::internal::kEmptyString) {
-        data_->clear();
-      }
-    }
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool GLOverlay::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_name;
-        break;
-      }
-      
-      // optional string name = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_name:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadString(
-                input, this->mutable_name()));
-          ::google::protobuf::internal::WireFormat::VerifyUTF8String(
-            this->name().data(), this->name().length(),
-            ::google::protobuf::internal::WireFormat::PARSE);
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(24)) goto parse_coordinateFrameType;
-        break;
-      }
-      
-      // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_coordinateFrameType:
-          int value;
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   int, ::google::protobuf::internal::WireFormatLite::TYPE_ENUM>(
-                 input, &value)));
-          if (::px::GLOverlay_CoordinateFrameType_IsValid(value)) {
-            set_coordinateframetype(static_cast< ::px::GLOverlay_CoordinateFrameType >(value));
-          } else {
-            mutable_unknown_fields()->AddVarint(3, value);
-          }
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(33)) goto parse_origin_x;
-        break;
-      }
-      
-      // optional double origin_x = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_origin_x:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &origin_x_)));
-          set_has_origin_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(41)) goto parse_origin_y;
-        break;
-      }
-      
-      // optional double origin_y = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_origin_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &origin_y_)));
-          set_has_origin_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(49)) goto parse_origin_z;
-        break;
-      }
-      
-      // optional double origin_z = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_origin_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &origin_z_)));
-          set_has_origin_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(58)) goto parse_data;
-        break;
-      }
-      
-      // optional bytes data = 7;
-      case 7: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_data:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_data()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void GLOverlay::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // optional string name = 2;
-  if (has_name()) {
-    ::google::protobuf::internal::WireFormat::VerifyUTF8String(
-      this->name().data(), this->name().length(),
-      ::google::protobuf::internal::WireFormat::SERIALIZE);
-    ::google::protobuf::internal::WireFormatLite::WriteString(
-      2, this->name(), output);
-  }
-  
-  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-  if (has_coordinateframetype()) {
-    ::google::protobuf::internal::WireFormatLite::WriteEnum(
-      3, this->coordinateframetype(), output);
-  }
-  
-  // optional double origin_x = 4;
-  if (has_origin_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->origin_x(), output);
-  }
-  
-  // optional double origin_y = 5;
-  if (has_origin_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->origin_y(), output);
-  }
-  
-  // optional double origin_z = 6;
-  if (has_origin_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->origin_z(), output);
-  }
-  
-  // optional bytes data = 7;
-  if (has_data()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      7, this->data(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* GLOverlay::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // optional string name = 2;
-  if (has_name()) {
-    ::google::protobuf::internal::WireFormat::VerifyUTF8String(
-      this->name().data(), this->name().length(),
-      ::google::protobuf::internal::WireFormat::SERIALIZE);
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteStringToArray(
-        2, this->name(), target);
-  }
-  
-  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-  if (has_coordinateframetype()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteEnumToArray(
-      3, this->coordinateframetype(), target);
-  }
-  
-  // optional double origin_x = 4;
-  if (has_origin_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->origin_x(), target);
-  }
-  
-  // optional double origin_y = 5;
-  if (has_origin_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->origin_y(), target);
-  }
-  
-  // optional double origin_z = 6;
-  if (has_origin_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->origin_z(), target);
-  }
-  
-  // optional bytes data = 7;
-  if (has_data()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        7, this->data(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int GLOverlay::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-    // optional string name = 2;
-    if (has_name()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::StringSize(
-          this->name());
-    }
-    
-    // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
-    if (has_coordinateframetype()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::EnumSize(this->coordinateframetype());
-    }
-    
-    // optional double origin_x = 4;
-    if (has_origin_x()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double origin_y = 5;
-    if (has_origin_y()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double origin_z = 6;
-    if (has_origin_z()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional bytes data = 7;
-    if (has_data()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->data());
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void GLOverlay::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const GLOverlay* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const GLOverlay*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void GLOverlay::MergeFrom(const GLOverlay& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-    if (from.has_name()) {
-      set_name(from.name());
-    }
-    if (from.has_coordinateframetype()) {
-      set_coordinateframetype(from.coordinateframetype());
-    }
-    if (from.has_origin_x()) {
-      set_origin_x(from.origin_x());
-    }
-    if (from.has_origin_y()) {
-      set_origin_y(from.origin_y());
-    }
-    if (from.has_origin_z()) {
-      set_origin_z(from.origin_z());
-    }
-    if (from.has_data()) {
-      set_data(from.data());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void GLOverlay::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void GLOverlay::CopyFrom(const GLOverlay& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool GLOverlay::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  return true;
-}
-
-void GLOverlay::Swap(GLOverlay* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    std::swap(name_, other->name_);
-    std::swap(coordinateframetype_, other->coordinateframetype_);
-    std::swap(origin_x_, other->origin_x_);
-    std::swap(origin_y_, other->origin_y_);
-    std::swap(origin_z_, other->origin_z_);
-    std::swap(data_, other->data_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata GLOverlay::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = GLOverlay_descriptor_;
-  metadata.reflection = GLOverlay_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int Obstacle::kXFieldNumber;
-const int Obstacle::kYFieldNumber;
-const int Obstacle::kZFieldNumber;
-const int Obstacle::kLengthFieldNumber;
-const int Obstacle::kWidthFieldNumber;
-const int Obstacle::kHeightFieldNumber;
-#endif  // !_MSC_VER
-
-Obstacle::Obstacle()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void Obstacle::InitAsDefaultInstance() {
-}
-
-Obstacle::Obstacle(const Obstacle& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void Obstacle::SharedCtor() {
-  _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  length_ = 0;
-  width_ = 0;
-  height_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-Obstacle::~Obstacle() {
-  SharedDtor();
-}
-
-void Obstacle::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void Obstacle::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* Obstacle::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return Obstacle_descriptor_;
-}
-
-const Obstacle& Obstacle::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-Obstacle* Obstacle::default_instance_ = NULL;
-
-Obstacle* Obstacle::New() const {
-  return new Obstacle;
-}
-
-void Obstacle::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    length_ = 0;
-    width_ = 0;
-    height_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool Obstacle::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // optional float x = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(21)) goto parse_y;
-        break;
-      }
-      
-      // optional float y = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_z;
-        break;
-      }
-      
-      // optional float z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(37)) goto parse_length;
-        break;
-      }
-      
-      // optional float length = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_length:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &length_)));
-          set_has_length();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(45)) goto parse_width;
-        break;
-      }
-      
-      // optional float width = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_width:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &width_)));
-          set_has_width();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(53)) goto parse_height;
-        break;
-      }
-      
-      // optional float height = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_height:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &height_)));
-          set_has_height();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void Obstacle::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // optional float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
-  }
-  
-  // optional float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
-  }
-  
-  // optional float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
-  }
-  
-  // optional float length = 4;
-  if (has_length()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->length(), output);
-  }
-  
-  // optional float width = 5;
-  if (has_width()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(5, this->width(), output);
-  }
-  
-  // optional float height = 6;
-  if (has_height()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(6, this->height(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* Obstacle::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // optional float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
-  }
-  
-  // optional float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
-  }
-  
-  // optional float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
-  }
-  
-  // optional float length = 4;
-  if (has_length()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->length(), target);
-  }
-  
-  // optional float width = 5;
-  if (has_width()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(5, this->width(), target);
-  }
-  
-  // optional float height = 6;
-  if (has_height()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(6, this->height(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int Obstacle::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // optional float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float length = 4;
-    if (has_length()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float width = 5;
-    if (has_width()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float height = 6;
-    if (has_height()) {
-      total_size += 1 + 4;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void Obstacle::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const Obstacle* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Obstacle*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void Obstacle::MergeFrom(const Obstacle& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_length()) {
-      set_length(from.length());
-    }
-    if (from.has_width()) {
-      set_width(from.width());
-    }
-    if (from.has_height()) {
-      set_height(from.height());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void Obstacle::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void Obstacle::CopyFrom(const Obstacle& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool Obstacle::IsInitialized() const {
-  
-  return true;
-}
-
-void Obstacle::Swap(Obstacle* other) {
-  if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(length_, other->length_);
-    std::swap(width_, other->width_);
-    std::swap(height_, other->height_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata Obstacle::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Obstacle_descriptor_;
-  metadata.reflection = Obstacle_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int ObstacleList::kHeaderFieldNumber;
-const int ObstacleList::kObstaclesFieldNumber;
-#endif  // !_MSC_VER
-
-ObstacleList::ObstacleList()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void ObstacleList::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-ObstacleList::ObstacleList(const ObstacleList& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void ObstacleList::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-ObstacleList::~ObstacleList() {
-  SharedDtor();
-}
-
-void ObstacleList::SharedDtor() {
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void ObstacleList::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* ObstacleList::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return ObstacleList_descriptor_;
-}
-
-const ObstacleList& ObstacleList::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-ObstacleList* ObstacleList::default_instance_ = NULL;
-
-ObstacleList* ObstacleList::New() const {
-  return new ObstacleList;
-}
-
-void ObstacleList::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-  }
-  obstacles_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool ObstacleList::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_obstacles;
-        break;
-      }
-      
-      // repeated .px.Obstacle obstacles = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_obstacles:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_obstacles()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_obstacles;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void ObstacleList::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // repeated .px.Obstacle obstacles = 2;
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->obstacles(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* ObstacleList::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // repeated .px.Obstacle obstacles = 2;
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->obstacles(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int ObstacleList::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-  }
-  // repeated .px.Obstacle obstacles = 2;
-  total_size += 1 * this->obstacles_size();
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->obstacles(i));
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void ObstacleList::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const ObstacleList* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const ObstacleList*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void ObstacleList::MergeFrom(const ObstacleList& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  obstacles_.MergeFrom(from.obstacles_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void ObstacleList::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void ObstacleList::CopyFrom(const ObstacleList& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool ObstacleList::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  return true;
-}
-
-void ObstacleList::Swap(ObstacleList* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    obstacles_.Swap(&other->obstacles_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata ObstacleList::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = ObstacleList_descriptor_;
-  metadata.reflection = ObstacleList_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int ObstacleMap::kHeaderFieldNumber;
-const int ObstacleMap::kTypeFieldNumber;
-const int ObstacleMap::kResolutionFieldNumber;
-const int ObstacleMap::kRowsFieldNumber;
-const int ObstacleMap::kColsFieldNumber;
-const int ObstacleMap::kMapR0FieldNumber;
-const int ObstacleMap::kMapC0FieldNumber;
-const int ObstacleMap::kArrayR0FieldNumber;
-const int ObstacleMap::kArrayC0FieldNumber;
-const int ObstacleMap::kDataFieldNumber;
-#endif  // !_MSC_VER
-
-ObstacleMap::ObstacleMap()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void ObstacleMap::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-ObstacleMap::ObstacleMap(const ObstacleMap& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void ObstacleMap::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  type_ = 0;
-  resolution_ = 0;
-  rows_ = 0;
-  cols_ = 0;
-  mapr0_ = 0;
-  mapc0_ = 0;
-  arrayr0_ = 0;
-  arrayc0_ = 0;
-  data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-ObstacleMap::~ObstacleMap() {
-  SharedDtor();
-}
-
-void ObstacleMap::SharedDtor() {
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    delete data_;
-  }
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void ObstacleMap::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* ObstacleMap::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return ObstacleMap_descriptor_;
-}
-
-const ObstacleMap& ObstacleMap::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-ObstacleMap* ObstacleMap::default_instance_ = NULL;
-
-ObstacleMap* ObstacleMap::New() const {
-  return new ObstacleMap;
-}
-
-void ObstacleMap::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-    type_ = 0;
-    resolution_ = 0;
-    rows_ = 0;
-    cols_ = 0;
-    mapr0_ = 0;
-    mapc0_ = 0;
-    arrayr0_ = 0;
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    arrayc0_ = 0;
-    if (has_data()) {
-      if (data_ != &::google::protobuf::internal::kEmptyString) {
-        data_->clear();
-      }
-    }
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool ObstacleMap::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(16)) goto parse_type;
-        break;
-      }
-      
-      // required int32 type = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &type_)));
-          set_has_type();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_resolution;
-        break;
-      }
-      
-      // optional float resolution = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_resolution:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &resolution_)));
-          set_has_resolution();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(32)) goto parse_rows;
-        break;
-      }
-      
-      // optional int32 rows = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_rows:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &rows_)));
-          set_has_rows();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(40)) goto parse_cols;
-        break;
-      }
-      
-      // optional int32 cols = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_cols:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &cols_)));
-          set_has_cols();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(48)) goto parse_mapR0;
-        break;
-      }
-      
-      // optional int32 mapR0 = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_mapR0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &mapr0_)));
-          set_has_mapr0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(56)) goto parse_mapC0;
-        break;
-      }
-      
-      // optional int32 mapC0 = 7;
-      case 7: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_mapC0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &mapc0_)));
-          set_has_mapc0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(64)) goto parse_arrayR0;
-        break;
-      }
-      
-      // optional int32 arrayR0 = 8;
-      case 8: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_arrayR0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &arrayr0_)));
-          set_has_arrayr0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(72)) goto parse_arrayC0;
-        break;
-      }
-      
-      // optional int32 arrayC0 = 9;
-      case 9: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_arrayC0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &arrayc0_)));
-          set_has_arrayc0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(82)) goto parse_data;
-        break;
-      }
-      
-      // optional bytes data = 10;
-      case 10: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_data:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_data()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void ObstacleMap::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // required int32 type = 2;
-  if (has_type()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->type(), output);
-  }
-  
-  // optional float resolution = 3;
-  if (has_resolution()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->resolution(), output);
-  }
-  
-  // optional int32 rows = 4;
-  if (has_rows()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->rows(), output);
-  }
-  
-  // optional int32 cols = 5;
-  if (has_cols()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(5, this->cols(), output);
-  }
-  
-  // optional int32 mapR0 = 6;
-  if (has_mapr0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(6, this->mapr0(), output);
-  }
-  
-  // optional int32 mapC0 = 7;
-  if (has_mapc0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(7, this->mapc0(), output);
-  }
-  
-  // optional int32 arrayR0 = 8;
-  if (has_arrayr0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(8, this->arrayr0(), output);
-  }
-  
-  // optional int32 arrayC0 = 9;
-  if (has_arrayc0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(9, this->arrayc0(), output);
-  }
-  
-  // optional bytes data = 10;
-  if (has_data()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      10, this->data(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* ObstacleMap::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // required int32 type = 2;
-  if (has_type()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->type(), target);
-  }
-  
-  // optional float resolution = 3;
-  if (has_resolution()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->resolution(), target);
-  }
-  
-  // optional int32 rows = 4;
-  if (has_rows()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->rows(), target);
-  }
-  
-  // optional int32 cols = 5;
-  if (has_cols()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(5, this->cols(), target);
-  }
-  
-  // optional int32 mapR0 = 6;
-  if (has_mapr0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(6, this->mapr0(), target);
-  }
-  
-  // optional int32 mapC0 = 7;
-  if (has_mapc0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(7, this->mapc0(), target);
-  }
-  
-  // optional int32 arrayR0 = 8;
-  if (has_arrayr0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(8, this->arrayr0(), target);
-  }
-  
-  // optional int32 arrayC0 = 9;
-  if (has_arrayc0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(9, this->arrayc0(), target);
-  }
-  
-  // optional bytes data = 10;
-  if (has_data()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        10, this->data(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int ObstacleMap::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-    // required int32 type = 2;
-    if (has_type()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->type());
-    }
-    
-    // optional float resolution = 3;
-    if (has_resolution()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional int32 rows = 4;
-    if (has_rows()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->rows());
-    }
-    
-    // optional int32 cols = 5;
-    if (has_cols()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->cols());
-    }
-    
-    // optional int32 mapR0 = 6;
-    if (has_mapr0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->mapr0());
-    }
-    
-    // optional int32 mapC0 = 7;
-    if (has_mapc0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->mapc0());
-    }
-    
-    // optional int32 arrayR0 = 8;
-    if (has_arrayr0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->arrayr0());
-    }
-    
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    // optional int32 arrayC0 = 9;
-    if (has_arrayc0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->arrayc0());
-    }
-    
-    // optional bytes data = 10;
-    if (has_data()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->data());
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void ObstacleMap::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const ObstacleMap* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const ObstacleMap*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void ObstacleMap::MergeFrom(const ObstacleMap& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-    if (from.has_type()) {
-      set_type(from.type());
-    }
-    if (from.has_resolution()) {
-      set_resolution(from.resolution());
-    }
-    if (from.has_rows()) {
-      set_rows(from.rows());
-    }
-    if (from.has_cols()) {
-      set_cols(from.cols());
-    }
-    if (from.has_mapr0()) {
-      set_mapr0(from.mapr0());
-    }
-    if (from.has_mapc0()) {
-      set_mapc0(from.mapc0());
-    }
-    if (from.has_arrayr0()) {
-      set_arrayr0(from.arrayr0());
-    }
-  }
-  if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (from.has_arrayc0()) {
-      set_arrayc0(from.arrayc0());
-    }
-    if (from.has_data()) {
-      set_data(from.data());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void ObstacleMap::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void ObstacleMap::CopyFrom(const ObstacleMap& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool ObstacleMap::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  return true;
-}
-
-void ObstacleMap::Swap(ObstacleMap* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    std::swap(type_, other->type_);
-    std::swap(resolution_, other->resolution_);
-    std::swap(rows_, other->rows_);
-    std::swap(cols_, other->cols_);
-    std::swap(mapr0_, other->mapr0_);
-    std::swap(mapc0_, other->mapc0_);
-    std::swap(arrayr0_, other->arrayr0_);
-    std::swap(arrayc0_, other->arrayc0_);
-    std::swap(data_, other->data_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata ObstacleMap::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = ObstacleMap_descriptor_;
-  metadata.reflection = ObstacleMap_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int Path::kHeaderFieldNumber;
-const int Path::kWaypointsFieldNumber;
-#endif  // !_MSC_VER
-
-Path::Path()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void Path::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-Path::Path(const Path& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void Path::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-Path::~Path() {
-  SharedDtor();
-}
-
-void Path::SharedDtor() {
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void Path::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* Path::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return Path_descriptor_;
-}
-
-const Path& Path::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-Path* Path::default_instance_ = NULL;
-
-Path* Path::New() const {
-  return new Path;
-}
-
-void Path::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-  }
-  waypoints_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool Path::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_waypoints;
-        break;
-      }
-      
-      // repeated .px.Waypoint waypoints = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_waypoints:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_waypoints()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_waypoints;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void Path::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // repeated .px.Waypoint waypoints = 2;
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->waypoints(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* Path::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // repeated .px.Waypoint waypoints = 2;
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->waypoints(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int Path::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-  }
-  // repeated .px.Waypoint waypoints = 2;
-  total_size += 1 * this->waypoints_size();
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->waypoints(i));
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void Path::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const Path* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Path*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void Path::MergeFrom(const Path& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  waypoints_.MergeFrom(from.waypoints_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void Path::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void Path::CopyFrom(const Path& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool Path::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  for (int i = 0; i < waypoints_size(); i++) {
-    if (!this->waypoints(i).IsInitialized()) return false;
-  }
-  return true;
-}
-
-void Path::Swap(Path* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    waypoints_.Swap(&other->waypoints_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata Path::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Path_descriptor_;
-  metadata.reflection = Path_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int PointCloudXYZI_PointXYZI::kXFieldNumber;
-const int PointCloudXYZI_PointXYZI::kYFieldNumber;
-const int PointCloudXYZI_PointXYZI::kZFieldNumber;
-const int PointCloudXYZI_PointXYZI::kIntensityFieldNumber;
-#endif  // !_MSC_VER
-
-PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void PointCloudXYZI_PointXYZI::InitAsDefaultInstance() {
-}
-
-PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI_PointXYZI::SharedCtor() {
-  _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  intensity_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-PointCloudXYZI_PointXYZI::~PointCloudXYZI_PointXYZI() {
-  SharedDtor();
-}
-
-void PointCloudXYZI_PointXYZI::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void PointCloudXYZI_PointXYZI::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZI_PointXYZI_descriptor_;
-}
-
-const PointCloudXYZI_PointXYZI& PointCloudXYZI_PointXYZI::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::default_instance_ = NULL;
-
-PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::New() const {
-  return new PointCloudXYZI_PointXYZI;
-}
-
-void PointCloudXYZI_PointXYZI::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    intensity_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool PointCloudXYZI_PointXYZI::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required float x = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(21)) goto parse_y;
-        break;
-      }
-      
-      // required float y = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_z;
-        break;
-      }
-      
-      // required float z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(37)) goto parse_intensity;
-        break;
-      }
-      
-      // required float intensity = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_intensity:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &intensity_)));
-          set_has_intensity();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void PointCloudXYZI_PointXYZI::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
-  }
-  
-  // required float intensity = 4;
-  if (has_intensity()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->intensity(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* PointCloudXYZI_PointXYZI::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
-  }
-  
-  // required float intensity = 4;
-  if (has_intensity()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->intensity(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int PointCloudXYZI_PointXYZI::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float intensity = 4;
-    if (has_intensity()) {
-      total_size += 1 + 4;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void PointCloudXYZI_PointXYZI::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZI_PointXYZI* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZI_PointXYZI*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void PointCloudXYZI_PointXYZI::MergeFrom(const PointCloudXYZI_PointXYZI& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_intensity()) {
-      set_intensity(from.intensity());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZI_PointXYZI::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI_PointXYZI::CopyFrom(const PointCloudXYZI_PointXYZI& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool PointCloudXYZI_PointXYZI::IsInitialized() const {
-  if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false;
-  
-  return true;
-}
-
-void PointCloudXYZI_PointXYZI::Swap(PointCloudXYZI_PointXYZI* other) {
-  if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(intensity_, other->intensity_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata PointCloudXYZI_PointXYZI::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZI_PointXYZI_descriptor_;
-  metadata.reflection = PointCloudXYZI_PointXYZI_reflection_;
-  return metadata;
-}
-
-
-// -------------------------------------------------------------------
-
-#ifndef _MSC_VER
-const int PointCloudXYZI::kHeaderFieldNumber;
-const int PointCloudXYZI::kPointsFieldNumber;
-#endif  // !_MSC_VER
-
-PointCloudXYZI::PointCloudXYZI()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void PointCloudXYZI::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-PointCloudXYZI::PointCloudXYZI(const PointCloudXYZI& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-PointCloudXYZI::~PointCloudXYZI() {
-  SharedDtor();
-}
-
-void PointCloudXYZI::SharedDtor() {
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void PointCloudXYZI::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* PointCloudXYZI::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZI_descriptor_;
-}
-
-const PointCloudXYZI& PointCloudXYZI::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-PointCloudXYZI* PointCloudXYZI::default_instance_ = NULL;
-
-PointCloudXYZI* PointCloudXYZI::New() const {
-  return new PointCloudXYZI;
-}
-
-void PointCloudXYZI::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-  }
-  points_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool PointCloudXYZI::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_points;
-        break;
-      }
-      
-      // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_points:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_points()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_points;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void PointCloudXYZI::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->points(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* PointCloudXYZI::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->points(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int PointCloudXYZI::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-  }
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  total_size += 1 * this->points_size();
-  for (int i = 0; i < this->points_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->points(i));
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void PointCloudXYZI::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZI* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZI*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void PointCloudXYZI::MergeFrom(const PointCloudXYZI& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  points_.MergeFrom(from.points_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZI::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI::CopyFrom(const PointCloudXYZI& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool PointCloudXYZI::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  for (int i = 0; i < points_size(); i++) {
-    if (!this->points(i).IsInitialized()) return false;
-  }
-  return true;
-}
-
-void PointCloudXYZI::Swap(PointCloudXYZI* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    points_.Swap(&other->points_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata PointCloudXYZI::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZI_descriptor_;
-  metadata.reflection = PointCloudXYZI_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int PointCloudXYZRGB_PointXYZRGB::kXFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kYFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kZFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kRgbFieldNumber;
-#endif  // !_MSC_VER
-
-PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void PointCloudXYZRGB_PointXYZRGB::InitAsDefaultInstance() {
-}
-
-PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void PointCloudXYZRGB_PointXYZRGB::SharedCtor() {
-  _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  rgb_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-PointCloudXYZRGB_PointXYZRGB::~PointCloudXYZRGB_PointXYZRGB() {
-  SharedDtor();
-}
-
-void PointCloudXYZRGB_PointXYZRGB::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void PointCloudXYZRGB_PointXYZRGB::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZRGB_PointXYZRGB_descriptor_;
-}
-
-const PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB_PointXYZRGB::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::default_instance_ = NULL;
-
-PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::New() const {
-  return new PointCloudXYZRGB_PointXYZRGB;
-}
-
-void PointCloudXYZRGB_PointXYZRGB::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    rgb_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool PointCloudXYZRGB_PointXYZRGB::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required float x = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(21)) goto parse_y;
-        break;
-      }
-      
-      // required float y = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_z;
-        break;
-      }
-      
-      // required float z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(37)) goto parse_rgb;
-        break;
-      }
-      
-      // required float rgb = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_rgb:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &rgb_)));
-          set_has_rgb();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
-  }
-  
-  // required float rgb = 4;
-  if (has_rgb()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->rgb(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
-  }
-  
-  // required float rgb = 4;
-  if (has_rgb()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->rgb(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int PointCloudXYZRGB_PointXYZRGB::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float rgb = 4;
-    if (has_rgb()) {
-      total_size += 1 + 4;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZRGB_PointXYZRGB* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZRGB_PointXYZRGB*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_rgb()) {
-      set_rgb(from.rgb());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool PointCloudXYZRGB_PointXYZRGB::IsInitialized() const {
-  if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false;
-  
-  return true;
-}
-
-void PointCloudXYZRGB_PointXYZRGB::Swap(PointCloudXYZRGB_PointXYZRGB* other) {
-  if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(rgb_, other->rgb_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata PointCloudXYZRGB_PointXYZRGB::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZRGB_PointXYZRGB_descriptor_;
-  metadata.reflection = PointCloudXYZRGB_PointXYZRGB_reflection_;
-  return metadata;
-}
-
-
-// -------------------------------------------------------------------
-
-#ifndef _MSC_VER
-const int PointCloudXYZRGB::kHeaderFieldNumber;
-const int PointCloudXYZRGB::kPointsFieldNumber;
-#endif  // !_MSC_VER
-
-PointCloudXYZRGB::PointCloudXYZRGB()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void PointCloudXYZRGB::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-PointCloudXYZRGB::PointCloudXYZRGB(const PointCloudXYZRGB& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void PointCloudXYZRGB::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-PointCloudXYZRGB::~PointCloudXYZRGB() {
-  SharedDtor();
-}
-
-void PointCloudXYZRGB::SharedDtor() {
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void PointCloudXYZRGB::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* PointCloudXYZRGB::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZRGB_descriptor_;
-}
-
-const PointCloudXYZRGB& PointCloudXYZRGB::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-PointCloudXYZRGB* PointCloudXYZRGB::default_instance_ = NULL;
-
-PointCloudXYZRGB* PointCloudXYZRGB::New() const {
-  return new PointCloudXYZRGB;
-}
-
-void PointCloudXYZRGB::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-  }
-  points_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool PointCloudXYZRGB::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_points;
-        break;
-      }
-      
-      // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_points:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_points()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(18)) goto parse_points;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void PointCloudXYZRGB::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->points(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* PointCloudXYZRGB::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->points(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int PointCloudXYZRGB::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-  }
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  total_size += 1 * this->points_size();
-  for (int i = 0; i < this->points_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->points(i));
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void PointCloudXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZRGB* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZRGB*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void PointCloudXYZRGB::MergeFrom(const PointCloudXYZRGB& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  points_.MergeFrom(from.points_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZRGB::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZRGB::CopyFrom(const PointCloudXYZRGB& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool PointCloudXYZRGB::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  for (int i = 0; i < points_size(); i++) {
-    if (!this->points(i).IsInitialized()) return false;
-  }
-  return true;
-}
-
-void PointCloudXYZRGB::Swap(PointCloudXYZRGB* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    points_.Swap(&other->points_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata PointCloudXYZRGB::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZRGB_descriptor_;
-  metadata.reflection = PointCloudXYZRGB_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int RGBDImage::kHeaderFieldNumber;
-const int RGBDImage::kColsFieldNumber;
-const int RGBDImage::kRowsFieldNumber;
-const int RGBDImage::kStep1FieldNumber;
-const int RGBDImage::kType1FieldNumber;
-const int RGBDImage::kImageData1FieldNumber;
-const int RGBDImage::kStep2FieldNumber;
-const int RGBDImage::kType2FieldNumber;
-const int RGBDImage::kImageData2FieldNumber;
-const int RGBDImage::kCameraConfigFieldNumber;
-const int RGBDImage::kCameraTypeFieldNumber;
-const int RGBDImage::kRollFieldNumber;
-const int RGBDImage::kPitchFieldNumber;
-const int RGBDImage::kYawFieldNumber;
-const int RGBDImage::kLonFieldNumber;
-const int RGBDImage::kLatFieldNumber;
-const int RGBDImage::kAltFieldNumber;
-const int RGBDImage::kGroundXFieldNumber;
-const int RGBDImage::kGroundYFieldNumber;
-const int RGBDImage::kGroundZFieldNumber;
-const int RGBDImage::kCameraMatrixFieldNumber;
-#endif  // !_MSC_VER
-
-RGBDImage::RGBDImage()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void RGBDImage::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
-}
-
-RGBDImage::RGBDImage(const RGBDImage& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void RGBDImage::SharedCtor() {
-  _cached_size_ = 0;
-  header_ = NULL;
-  cols_ = 0u;
-  rows_ = 0u;
-  step1_ = 0u;
-  type1_ = 0u;
-  imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  step2_ = 0u;
-  type2_ = 0u;
-  imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  camera_config_ = 0u;
-  camera_type_ = 0u;
-  roll_ = 0;
-  pitch_ = 0;
-  yaw_ = 0;
-  lon_ = 0;
-  lat_ = 0;
-  alt_ = 0;
-  ground_x_ = 0;
-  ground_y_ = 0;
-  ground_z_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-RGBDImage::~RGBDImage() {
-  SharedDtor();
-}
-
-void RGBDImage::SharedDtor() {
-  if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-    delete imagedata1_;
-  }
-  if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-    delete imagedata2_;
-  }
-  if (this != default_instance_) {
-    delete header_;
-  }
-}
-
-void RGBDImage::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* RGBDImage::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return RGBDImage_descriptor_;
-}
-
-const RGBDImage& RGBDImage::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-RGBDImage* RGBDImage::default_instance_ = NULL;
-
-RGBDImage* RGBDImage::New() const {
-  return new RGBDImage;
-}
-
-void RGBDImage::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-    cols_ = 0u;
-    rows_ = 0u;
-    step1_ = 0u;
-    type1_ = 0u;
-    if (has_imagedata1()) {
-      if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-        imagedata1_->clear();
-      }
-    }
-    step2_ = 0u;
-    type2_ = 0u;
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (has_imagedata2()) {
-      if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-        imagedata2_->clear();
-      }
-    }
-    camera_config_ = 0u;
-    camera_type_ = 0u;
-    roll_ = 0;
-    pitch_ = 0;
-    yaw_ = 0;
-    lon_ = 0;
-    lat_ = 0;
-  }
-  if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    alt_ = 0;
-    ground_x_ = 0;
-    ground_y_ = 0;
-    ground_z_ = 0;
-  }
-  camera_matrix_.Clear();
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool RGBDImage::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(16)) goto parse_cols;
-        break;
-      }
-      
-      // required uint32 cols = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_cols:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &cols_)));
-          set_has_cols();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(24)) goto parse_rows;
-        break;
-      }
-      
-      // required uint32 rows = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_rows:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &rows_)));
-          set_has_rows();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(32)) goto parse_step1;
-        break;
-      }
-      
-      // required uint32 step1 = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_step1:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &step1_)));
-          set_has_step1();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(40)) goto parse_type1;
-        break;
-      }
-      
-      // required uint32 type1 = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type1:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &type1_)));
-          set_has_type1();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(50)) goto parse_imageData1;
-        break;
-      }
-      
-      // required bytes imageData1 = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_imageData1:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_imagedata1()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(56)) goto parse_step2;
-        break;
-      }
-      
-      // required uint32 step2 = 7;
-      case 7: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_step2:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &step2_)));
-          set_has_step2();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(64)) goto parse_type2;
-        break;
-      }
-      
-      // required uint32 type2 = 8;
-      case 8: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type2:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &type2_)));
-          set_has_type2();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(74)) goto parse_imageData2;
-        break;
-      }
-      
-      // required bytes imageData2 = 9;
-      case 9: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_imageData2:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_imagedata2()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(80)) goto parse_camera_config;
-        break;
-      }
-      
-      // optional uint32 camera_config = 10;
-      case 10: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_camera_config:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &camera_config_)));
-          set_has_camera_config();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(88)) goto parse_camera_type;
-        break;
-      }
-      
-      // optional uint32 camera_type = 11;
-      case 11: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_camera_type:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &camera_type_)));
-          set_has_camera_type();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(101)) goto parse_roll;
-        break;
-      }
-      
-      // optional float roll = 12;
-      case 12: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_roll:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &roll_)));
-          set_has_roll();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(109)) goto parse_pitch;
-        break;
-      }
-      
-      // optional float pitch = 13;
-      case 13: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_pitch:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &pitch_)));
-          set_has_pitch();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(117)) goto parse_yaw;
-        break;
-      }
-      
-      // optional float yaw = 14;
-      case 14: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_yaw:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &yaw_)));
-          set_has_yaw();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(125)) goto parse_lon;
-        break;
-      }
-      
-      // optional float lon = 15;
-      case 15: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_lon:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &lon_)));
-          set_has_lon();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(133)) goto parse_lat;
-        break;
-      }
-      
-      // optional float lat = 16;
-      case 16: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_lat:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &lat_)));
-          set_has_lat();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(141)) goto parse_alt;
-        break;
-      }
-      
-      // optional float alt = 17;
-      case 17: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_alt:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &alt_)));
-          set_has_alt();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(149)) goto parse_ground_x;
-        break;
-      }
-      
-      // optional float ground_x = 18;
-      case 18: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_x:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_x_)));
-          set_has_ground_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(157)) goto parse_ground_y;
-        break;
-      }
-      
-      // optional float ground_y = 19;
-      case 19: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_y_)));
-          set_has_ground_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(165)) goto parse_ground_z;
-        break;
-      }
-      
-      // optional float ground_z = 20;
-      case 20: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_z_)));
-          set_has_ground_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(173)) goto parse_camera_matrix;
-        break;
-      }
-      
-      // repeated float camera_matrix = 21;
-      case 21: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_camera_matrix:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadRepeatedPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 2, 173, input, this->mutable_camera_matrix())));
-        } else if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag)
-                   == ::google::protobuf::internal::WireFormatLite::
-                      WIRETYPE_LENGTH_DELIMITED) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPackedPrimitiveNoInline<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, this->mutable_camera_matrix())));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(173)) goto parse_camera_matrix;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void RGBDImage::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
-  }
-  
-  // required uint32 cols = 2;
-  if (has_cols()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(2, this->cols(), output);
-  }
-  
-  // required uint32 rows = 3;
-  if (has_rows()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(3, this->rows(), output);
-  }
-  
-  // required uint32 step1 = 4;
-  if (has_step1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(4, this->step1(), output);
-  }
-  
-  // required uint32 type1 = 5;
-  if (has_type1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(5, this->type1(), output);
-  }
-  
-  // required bytes imageData1 = 6;
-  if (has_imagedata1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      6, this->imagedata1(), output);
-  }
-  
-  // required uint32 step2 = 7;
-  if (has_step2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(7, this->step2(), output);
-  }
-  
-  // required uint32 type2 = 8;
-  if (has_type2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(8, this->type2(), output);
-  }
-  
-  // required bytes imageData2 = 9;
-  if (has_imagedata2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      9, this->imagedata2(), output);
-  }
-  
-  // optional uint32 camera_config = 10;
-  if (has_camera_config()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(10, this->camera_config(), output);
-  }
-  
-  // optional uint32 camera_type = 11;
-  if (has_camera_type()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(11, this->camera_type(), output);
-  }
-  
-  // optional float roll = 12;
-  if (has_roll()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(12, this->roll(), output);
-  }
-  
-  // optional float pitch = 13;
-  if (has_pitch()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(13, this->pitch(), output);
-  }
-  
-  // optional float yaw = 14;
-  if (has_yaw()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(14, this->yaw(), output);
-  }
-  
-  // optional float lon = 15;
-  if (has_lon()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(15, this->lon(), output);
-  }
-  
-  // optional float lat = 16;
-  if (has_lat()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(16, this->lat(), output);
-  }
-  
-  // optional float alt = 17;
-  if (has_alt()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(17, this->alt(), output);
-  }
-  
-  // optional float ground_x = 18;
-  if (has_ground_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(18, this->ground_x(), output);
-  }
-  
-  // optional float ground_y = 19;
-  if (has_ground_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(19, this->ground_y(), output);
-  }
-  
-  // optional float ground_z = 20;
-  if (has_ground_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(20, this->ground_z(), output);
-  }
-  
-  // repeated float camera_matrix = 21;
-  for (int i = 0; i < this->camera_matrix_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(
-      21, this->camera_matrix(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* RGBDImage::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // required uint32 cols = 2;
-  if (has_cols()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(2, this->cols(), target);
-  }
-  
-  // required uint32 rows = 3;
-  if (has_rows()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(3, this->rows(), target);
-  }
-  
-  // required uint32 step1 = 4;
-  if (has_step1()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(4, this->step1(), target);
-  }
-  
-  // required uint32 type1 = 5;
-  if (has_type1()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(5, this->type1(), target);
-  }
-  
-  // required bytes imageData1 = 6;
-  if (has_imagedata1()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        6, this->imagedata1(), target);
-  }
-  
-  // required uint32 step2 = 7;
-  if (has_step2()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(7, this->step2(), target);
-  }
-  
-  // required uint32 type2 = 8;
-  if (has_type2()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(8, this->type2(), target);
-  }
-  
-  // required bytes imageData2 = 9;
-  if (has_imagedata2()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        9, this->imagedata2(), target);
-  }
-  
-  // optional uint32 camera_config = 10;
-  if (has_camera_config()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(10, this->camera_config(), target);
-  }
-  
-  // optional uint32 camera_type = 11;
-  if (has_camera_type()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(11, this->camera_type(), target);
-  }
-  
-  // optional float roll = 12;
-  if (has_roll()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(12, this->roll(), target);
-  }
-  
-  // optional float pitch = 13;
-  if (has_pitch()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(13, this->pitch(), target);
-  }
-  
-  // optional float yaw = 14;
-  if (has_yaw()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(14, this->yaw(), target);
-  }
-  
-  // optional float lon = 15;
-  if (has_lon()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(15, this->lon(), target);
-  }
-  
-  // optional float lat = 16;
-  if (has_lat()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(16, this->lat(), target);
-  }
-  
-  // optional float alt = 17;
-  if (has_alt()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(17, this->alt(), target);
-  }
-  
-  // optional float ground_x = 18;
-  if (has_ground_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(18, this->ground_x(), target);
-  }
-  
-  // optional float ground_y = 19;
-  if (has_ground_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(19, this->ground_y(), target);
-  }
-  
-  // optional float ground_z = 20;
-  if (has_ground_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(20, this->ground_z(), target);
-  }
-  
-  // repeated float camera_matrix = 21;
-  for (int i = 0; i < this->camera_matrix_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteFloatToArray(21, this->camera_matrix(i), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int RGBDImage::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-    // required uint32 cols = 2;
-    if (has_cols()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->cols());
-    }
-    
-    // required uint32 rows = 3;
-    if (has_rows()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->rows());
-    }
-    
-    // required uint32 step1 = 4;
-    if (has_step1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->step1());
-    }
-    
-    // required uint32 type1 = 5;
-    if (has_type1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->type1());
-    }
-    
-    // required bytes imageData1 = 6;
-    if (has_imagedata1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->imagedata1());
-    }
-    
-    // required uint32 step2 = 7;
-    if (has_step2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->step2());
-    }
-    
-    // required uint32 type2 = 8;
-    if (has_type2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->type2());
-    }
-    
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    // required bytes imageData2 = 9;
-    if (has_imagedata2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->imagedata2());
-    }
-    
-    // optional uint32 camera_config = 10;
-    if (has_camera_config()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->camera_config());
-    }
-    
-    // optional uint32 camera_type = 11;
-    if (has_camera_type()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->camera_type());
-    }
-    
-    // optional float roll = 12;
-    if (has_roll()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float pitch = 13;
-    if (has_pitch()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float yaw = 14;
-    if (has_yaw()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float lon = 15;
-    if (has_lon()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float lat = 16;
-    if (has_lat()) {
-      total_size += 2 + 4;
-    }
-    
-  }
-  if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    // optional float alt = 17;
-    if (has_alt()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_x = 18;
-    if (has_ground_x()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_y = 19;
-    if (has_ground_y()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_z = 20;
-    if (has_ground_z()) {
-      total_size += 2 + 4;
-    }
-    
-  }
-  // repeated float camera_matrix = 21;
-  {
-    int data_size = 0;
-    data_size = 4 * this->camera_matrix_size();
-    total_size += 2 * this->camera_matrix_size() + data_size;
-  }
-  
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void RGBDImage::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const RGBDImage* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const RGBDImage*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void RGBDImage::MergeFrom(const RGBDImage& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  camera_matrix_.MergeFrom(from.camera_matrix_);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-    if (from.has_cols()) {
-      set_cols(from.cols());
-    }
-    if (from.has_rows()) {
-      set_rows(from.rows());
-    }
-    if (from.has_step1()) {
-      set_step1(from.step1());
-    }
-    if (from.has_type1()) {
-      set_type1(from.type1());
-    }
-    if (from.has_imagedata1()) {
-      set_imagedata1(from.imagedata1());
-    }
-    if (from.has_step2()) {
-      set_step2(from.step2());
-    }
-    if (from.has_type2()) {
-      set_type2(from.type2());
-    }
-  }
-  if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (from.has_imagedata2()) {
-      set_imagedata2(from.imagedata2());
-    }
-    if (from.has_camera_config()) {
-      set_camera_config(from.camera_config());
-    }
-    if (from.has_camera_type()) {
-      set_camera_type(from.camera_type());
-    }
-    if (from.has_roll()) {
-      set_roll(from.roll());
-    }
-    if (from.has_pitch()) {
-      set_pitch(from.pitch());
-    }
-    if (from.has_yaw()) {
-      set_yaw(from.yaw());
-    }
-    if (from.has_lon()) {
-      set_lon(from.lon());
-    }
-    if (from.has_lat()) {
-      set_lat(from.lat());
-    }
-  }
-  if (from._has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    if (from.has_alt()) {
-      set_alt(from.alt());
-    }
-    if (from.has_ground_x()) {
-      set_ground_x(from.ground_x());
-    }
-    if (from.has_ground_y()) {
-      set_ground_y(from.ground_y());
-    }
-    if (from.has_ground_z()) {
-      set_ground_z(from.ground_z());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void RGBDImage::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void RGBDImage::CopyFrom(const RGBDImage& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool RGBDImage::IsInitialized() const {
-  if ((_has_bits_[0] & 0x000001ff) != 0x000001ff) return false;
-  
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  return true;
-}
-
-void RGBDImage::Swap(RGBDImage* other) {
-  if (other != this) {
-    std::swap(header_, other->header_);
-    std::swap(cols_, other->cols_);
-    std::swap(rows_, other->rows_);
-    std::swap(step1_, other->step1_);
-    std::swap(type1_, other->type1_);
-    std::swap(imagedata1_, other->imagedata1_);
-    std::swap(step2_, other->step2_);
-    std::swap(type2_, other->type2_);
-    std::swap(imagedata2_, other->imagedata2_);
-    std::swap(camera_config_, other->camera_config_);
-    std::swap(camera_type_, other->camera_type_);
-    std::swap(roll_, other->roll_);
-    std::swap(pitch_, other->pitch_);
-    std::swap(yaw_, other->yaw_);
-    std::swap(lon_, other->lon_);
-    std::swap(lat_, other->lat_);
-    std::swap(alt_, other->alt_);
-    std::swap(ground_x_, other->ground_x_);
-    std::swap(ground_y_, other->ground_y_);
-    std::swap(ground_z_, other->ground_z_);
-    camera_matrix_.Swap(&other->camera_matrix_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata RGBDImage::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = RGBDImage_descriptor_;
-  metadata.reflection = RGBDImage_reflection_;
-  return metadata;
-}
-
-
-// ===================================================================
-
-#ifndef _MSC_VER
-const int Waypoint::kXFieldNumber;
-const int Waypoint::kYFieldNumber;
-const int Waypoint::kZFieldNumber;
-const int Waypoint::kRollFieldNumber;
-const int Waypoint::kPitchFieldNumber;
-const int Waypoint::kYawFieldNumber;
-#endif  // !_MSC_VER
-
-Waypoint::Waypoint()
-  : ::google::protobuf::Message() {
-  SharedCtor();
-}
-
-void Waypoint::InitAsDefaultInstance() {
-}
-
-Waypoint::Waypoint(const Waypoint& from)
-  : ::google::protobuf::Message() {
-  SharedCtor();
-  MergeFrom(from);
-}
-
-void Waypoint::SharedCtor() {
-  _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  roll_ = 0;
-  pitch_ = 0;
-  yaw_ = 0;
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-}
-
-Waypoint::~Waypoint() {
-  SharedDtor();
-}
-
-void Waypoint::SharedDtor() {
-  if (this != default_instance_) {
-  }
-}
-
-void Waypoint::SetCachedSize(int size) const {
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-}
-const ::google::protobuf::Descriptor* Waypoint::descriptor() {
-  protobuf_AssignDescriptorsOnce();
-  return Waypoint_descriptor_;
-}
-
-const Waypoint& Waypoint::default_instance() {
-  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
-}
-
-Waypoint* Waypoint::default_instance_ = NULL;
-
-Waypoint* Waypoint::New() const {
-  return new Waypoint;
-}
-
-void Waypoint::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    roll_ = 0;
-    pitch_ = 0;
-    yaw_ = 0;
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
-}
-
-bool Waypoint::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required double x = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &x_)));
-          set_has_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(17)) goto parse_y;
-        break;
-      }
-      
-      // required double y = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(25)) goto parse_z;
-        break;
-      }
-      
-      // optional double z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(33)) goto parse_roll;
-        break;
-      }
-      
-      // optional double roll = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_roll:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &roll_)));
-          set_has_roll();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(41)) goto parse_pitch;
-        break;
-      }
-      
-      // optional double pitch = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_pitch:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &pitch_)));
-          set_has_pitch();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(49)) goto parse_yaw;
-        break;
-      }
-      
-      // optional double yaw = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_yaw:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &yaw_)));
-          set_has_yaw();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
-        break;
-      }
-    }
-  }
-  return true;
-#undef DO_
-}
-
-void Waypoint::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required double x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(1, this->x(), output);
-  }
-  
-  // required double y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(2, this->y(), output);
-  }
-  
-  // optional double z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->z(), output);
-  }
-  
-  // optional double roll = 4;
-  if (has_roll()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->roll(), output);
-  }
-  
-  // optional double pitch = 5;
-  if (has_pitch()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->pitch(), output);
-  }
-  
-  // optional double yaw = 6;
-  if (has_yaw()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->yaw(), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
-  }
-}
-
-::google::protobuf::uint8* Waypoint::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required double x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(1, this->x(), target);
-  }
-  
-  // required double y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(2, this->y(), target);
-  }
-  
-  // optional double z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->z(), target);
-  }
-  
-  // optional double roll = 4;
-  if (has_roll()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->roll(), target);
-  }
-  
-  // optional double pitch = 5;
-  if (has_pitch()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->pitch(), target);
-  }
-  
-  // optional double yaw = 6;
-  if (has_yaw()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->yaw(), target);
-  }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
-  }
-  return target;
-}
-
-int Waypoint::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required double x = 1;
-    if (has_x()) {
-      total_size += 1 + 8;
-    }
-    
-    // required double y = 2;
-    if (has_y()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double z = 3;
-    if (has_z()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double roll = 4;
-    if (has_roll()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double pitch = 5;
-    if (has_pitch()) {
-      total_size += 1 + 8;
-    }
-    
-    // optional double yaw = 6;
-    if (has_yaw()) {
-      total_size += 1 + 8;
-    }
-    
-  }
-  if (!unknown_fields().empty()) {
-    total_size +=
-      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
-        unknown_fields());
-  }
-  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
-  _cached_size_ = total_size;
-  GOOGLE_SAFE_CONCURRENT_WRITES_END();
-  return total_size;
-}
-
-void Waypoint::MergeFrom(const ::google::protobuf::Message& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  const Waypoint* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Waypoint*>(
-      &from);
-  if (source == NULL) {
-    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
-  } else {
-    MergeFrom(*source);
-  }
-}
-
-void Waypoint::MergeFrom(const Waypoint& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_roll()) {
-      set_roll(from.roll());
-    }
-    if (from.has_pitch()) {
-      set_pitch(from.pitch());
-    }
-    if (from.has_yaw()) {
-      set_yaw(from.yaw());
-    }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void Waypoint::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void Waypoint::CopyFrom(const Waypoint& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool Waypoint::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false;
-  
-  return true;
-}
-
-void Waypoint::Swap(Waypoint* other) {
-  if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(roll_, other->roll_);
-    std::swap(pitch_, other->pitch_);
-    std::swap(yaw_, other->yaw_);
-    std::swap(_has_bits_[0], other->_has_bits_[0]);
-    _unknown_fields_.Swap(&other->_unknown_fields_);
-    std::swap(_cached_size_, other->_cached_size_);
-  }
-}
-
-::google::protobuf::Metadata Waypoint::GetMetadata() const {
-  protobuf_AssignDescriptorsOnce();
-  ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Waypoint_descriptor_;
-  metadata.reflection = Waypoint_reflection_;
-  return metadata;
-}
-
-
-// @@protoc_insertion_point(namespace_scope)
-
-}  // namespace px
-
-// @@protoc_insertion_point(global_scope)
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore b/mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore
deleted file mode 100644
index 7c98650cc..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-/testmav0.9
-/testmav1.0
-/testmav1.0_nonstrict
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c b/mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c
deleted file mode 100644
index 2fd7fa378..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c
+++ /dev/null
@@ -1,159 +0,0 @@
-/*
-  simple MAVLink testsuite for C
- */
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <assert.h>
-#include <stddef.h>
-
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
-#define MAVLINK_COMM_NUM_BUFFERS 2
-
-// this trick allows us to make mavlink_message_t as small as possible
-// for this dialect, which saves some memory
-#include <version.h>
-#define MAVLINK_MAX_PAYLOAD_LEN MAVLINK_MAX_DIALECT_PAYLOAD_SIZE
-
-#include <mavlink_types.h>
-static mavlink_system_t mavlink_system = {42,11,};
-
-#define MAVLINK_ASSERT(x) assert(x)
-static void comm_send_ch(mavlink_channel_t chan, uint8_t c);
-
-static mavlink_message_t last_msg;
-
-#include <mavlink.h>
-#include <testsuite.h>
-
-static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS];
-
-static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
-static unsigned error_count;
-
-static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO;
-
-static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx)
-{
-#define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def)
-	switch (f->type) {
-	case MAVLINK_TYPE_CHAR:
-		printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1));
-		break;
-	case MAVLINK_TYPE_UINT8_T:
-		printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1));
-		break;
-	case MAVLINK_TYPE_INT8_T:
-		printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1));
-		break;
-	case MAVLINK_TYPE_UINT16_T:
-		printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2));
-		break;
-	case MAVLINK_TYPE_INT16_T:
-		printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2));
-		break;
-	case MAVLINK_TYPE_UINT32_T:
-		printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4));
-		break;
-	case MAVLINK_TYPE_INT32_T:
-		printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4));
-		break;
-	case MAVLINK_TYPE_UINT64_T:
-		printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8));
-		break;
-	case MAVLINK_TYPE_INT64_T:
-		printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8));
-		break;
-	case MAVLINK_TYPE_FLOAT:
-		printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4));
-		break;
-	case MAVLINK_TYPE_DOUBLE:
-		printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8));
-		break;
-	}
-}
-
-static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f)
-{
-	printf("%s: ", f->name);
-	if (f->array_length == 0) {
-		print_one_field(msg, f, 0);
-		printf(" ");
-	} else {
-		unsigned i;
-		/* print an array */
-		if (f->type == MAVLINK_TYPE_CHAR) {
-			printf("'%.*s'", f->array_length,
-			       f->wire_offset+(const char *)_MAV_PAYLOAD(msg));
-			
-		} else {
-			printf("[ ");
-			for (i=0; i<f->array_length; i++) {
-				print_one_field(msg, f, i);
-				if (i < f->array_length) {
-					printf(", ");
-				}
-			}
-			printf("]");
-		}
-	}
-	printf(" ");
-}
-
-static void print_message(mavlink_message_t *msg)
-{
-	const mavlink_message_info_t *m = &message_info[msg->msgid];
-	const mavlink_field_info_t *f = m->fields;
-	unsigned i;
-	printf("%s { ", m->name);
-	for (i=0; i<m->num_fields; i++) {
-		print_field(msg, &f[i]);
-	}
-	printf("}\n");
-}
-
-static void comm_send_ch(mavlink_channel_t chan, uint8_t c)
-{
-	mavlink_status_t status;
-	if (mavlink_parse_char(chan, c, &last_msg, &status)) {
-		print_message(&last_msg);
-		chan_counts[chan]++;
-		/* channel 0 gets 3 messages per message, because of
-		   the channel defaults for _pack() and _encode() */
-		if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) {
-			printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", 
-			       chan_counts[chan], status.current_rx_seq);
-			error_count++;
-		} else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) {
-			printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", 
-			       (unsigned)chan, chan_counts[chan], status.current_rx_seq);
-			error_count++;
-		}
-		if (message_lengths[last_msg.msgid] != last_msg.len) {
-			printf("Incorrect message length %u for message %u - expected %u\n", 
-			       (unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]);
-			error_count++;
-		}
-	}
-	if (status.packet_rx_drop_count != 0) {
-		printf("Parse error at packet %u\n", chan_counts[chan]);
-		error_count++;
-	}
-}
-
-int main(void)
-{
-	mavlink_channel_t chan;
-	mavlink_test_all(11, 10, &last_msg);
-	for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) {
-		printf("Received %u messages on channel %u OK\n", 
-		       chan_counts[chan], (unsigned)chan);
-	}
-	if (error_count != 0) {
-		printf("Error count %u\n", error_count);
-		exit(1);
-	}
-	printf("No errors detected\n");
-	return 0;
-}
-
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp b/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp
deleted file mode 100644
index 98b4abf05..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-// stdafx.cpp : source file that includes just the standard includes

-// testmav.pch will be the pre-compiled header

-// stdafx.obj will contain the pre-compiled type information

-

-#include "stdafx.h"

-

-// TODO: reference any additional headers you need in STDAFX.H

-// and not in this file

diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h b/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h
deleted file mode 100644
index 47a0d0252..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h
+++ /dev/null
@@ -1,15 +0,0 @@
-// stdafx.h : include file for standard system include files,

-// or project specific include files that are used frequently, but

-// are changed infrequently

-//

-

-#pragma once

-

-#include "targetver.h"

-

-#include <stdio.h>

-#include <tchar.h>

-

-

-

-// TODO: reference additional headers your program requires here

diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h b/mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h
deleted file mode 100644
index 90e767bfc..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#pragma once

-

-// Including SDKDDKVer.h defines the highest available Windows platform.

-

-// If you wish to build your application for a previous Windows platform, include WinSDKVer.h and

-// set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h.

-

-#include <SDKDDKVer.h>

diff --git a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp b/mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp
deleted file mode 100644
index aa83caced..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp
+++ /dev/null
@@ -1,154 +0,0 @@
-// testmav.cpp : Defines the entry point for the console application.

-//

-

-#include "stdafx.h"

-#include "stdio.h"

-#include "stdint.h"

-#include "stddef.h"

-#include "assert.h"

-

-

-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS

-#define MAVLINK_COMM_NUM_BUFFERS 2

-

-#include <mavlink_types.h>

-static mavlink_system_t mavlink_system = {42,11,};

-

-#define MAVLINK_ASSERT(x) assert(x)

-static void comm_send_ch(mavlink_channel_t chan, uint8_t c);

-

-static mavlink_message_t last_msg;

-

-#include <common/mavlink.h>

-#include <common/testsuite.h>

-

-static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS];

-

-static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS;

-static unsigned error_count;

-

-static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO;

-

-static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx)

-{

-#define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def)

-	switch (f->type) {

-	case MAVLINK_TYPE_CHAR:

-		printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1));

-		break;

-	case MAVLINK_TYPE_UINT8_T:

-		printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1));

-		break;

-	case MAVLINK_TYPE_INT8_T:

-		printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1));

-		break;

-	case MAVLINK_TYPE_UINT16_T:

-		printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2));

-		break;

-	case MAVLINK_TYPE_INT16_T:

-		printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2));

-		break;

-	case MAVLINK_TYPE_UINT32_T:

-		printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4));

-		break;

-	case MAVLINK_TYPE_INT32_T:

-		printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4));

-		break;

-	case MAVLINK_TYPE_UINT64_T:

-		printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8));

-		break;

-	case MAVLINK_TYPE_INT64_T:

-		printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8));

-		break;

-	case MAVLINK_TYPE_FLOAT:

-		printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4));

-		break;

-	case MAVLINK_TYPE_DOUBLE:

-		printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8));

-		break;

-	}

-}

-

-static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f)

-{

-	printf("%s: ", f->name);

-	if (f->array_length == 0) {

-		print_one_field(msg, f, 0);

-		printf(" ");

-	} else {

-		unsigned i;

-		/* print an array */

-		if (f->type == MAVLINK_TYPE_CHAR) {

-			printf("'%.*s'", f->array_length,

-			       f->wire_offset+(const char *)_MAV_PAYLOAD(msg));

-			

-		} else {

-			printf("[ ");

-			for (i=0; i<f->array_length; i++) {

-				print_one_field(msg, f, i);

-				if (i < f->array_length) {

-					printf(", ");

-				}

-			}

-			printf("]");

-		}

-	}

-	printf(" ");

-}

-

-static void print_message(mavlink_message_t *msg)

-{

-	const mavlink_message_info_t *m = &message_info[msg->msgid];

-	const mavlink_field_info_t *f = m->fields;

-	unsigned i;

-	printf("%s { ", m->name);

-	for (i=0; i<m->num_fields; i++) {

-		print_field(msg, &f[i]);

-	}

-	printf("}\n");

-}

-

-static void comm_send_ch(mavlink_channel_t chan, uint8_t c)

-{

-	mavlink_status_t status;

-	if (mavlink_parse_char(chan, c, &last_msg, &status)) {

-		print_message(&last_msg);

-		chan_counts[chan]++;

-		/* channel 0 gets 3 messages per message, because of

-		   the channel defaults for _pack() and _encode() */

-		if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) {

-			printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", 

-			       chan_counts[chan], status.current_rx_seq);

-			error_count++;

-		} else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) {

-			printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", 

-			       (unsigned)chan, chan_counts[chan], status.current_rx_seq);

-			error_count++;

-		}

-		if (message_lengths[last_msg.msgid] != last_msg.len) {

-			printf("Incorrect message length %u for message %u - expected %u\n", 

-			       (unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]);

-			error_count++;

-		}

-	}

-	if (status.packet_rx_drop_count != 0) {

-		printf("Parse error at packet %u\n", chan_counts[chan]);

-		error_count++;

-	}

-}

-	

-int _tmain(int argc, _TCHAR* argv[])

-{

-	int chan;

-	mavlink_test_all(11, 10, &last_msg);

-	for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) {

-		printf("Received %u messages on channel %u OK\n", 

-		       chan_counts[chan], (unsigned)chan);

-	}

-	if (error_count != 0) {

-		printf("Error count %u\n", error_count);

-		return(1);

-	}

-	printf("No errors detected\n");

-	return 0;

-}

diff --git a/mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py b/mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py
deleted file mode 100644
index 165c1b343..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py
+++ /dev/null
@@ -1,93 +0,0 @@
-#!/usr/bin/env python
-'''
-Use mavgen.py matrixpilot.xml definitions to generate
-C and Python MAVLink routines for sending and parsing the protocol
-This python script is soley for MatrixPilot MAVLink impoementation
-
-Copyright Pete Hollands 2011
-Released under GNU GPL version 3 or later
-'''
-
-import os, sys, glob, re
-from shutil import copy
-from mavgen import mavgen
-
-# allow import from the parent directory, where mavutil.py is
-# Under Windows, this script must be run from a DOS command window
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-class options:
-    """ a class to simulate the options of mavgen OptionsParser"""
-    def __init__(self, lang, output, wire_protocol):
-        self.language = lang
-        self.wire_protocol = wire_protocol
-        self.output = output
-
-def remove_include_files(target_directory):
-    search_pattern = target_directory+'/*.h'
-    print "search pattern is", search_pattern
-    files_to_remove = glob.glob(search_pattern)
-    for afile in files_to_remove :
-        try:
-            print "removing", afile
-            os.remove(afile)
-        except:
-            print "error while trying to remove", afile
-
-def copy_include_files(source_directory,target_directory):
-    search_pattern = source_directory+'/*.h'
-    files_to_copy = glob.glob(search_pattern)
-    for afile in files_to_copy:
-        basename = os.path.basename(afile)
-        print "Copying ...", basename
-        copy(afile, target_directory)
-
-protocol = "1.0"
-
-xml_directory = './message_definitions/v'+protocol
-print "xml_directory is", xml_directory
-xml_file_names = []
-xml_file_names.append(xml_directory+"/"+"matrixpilot.xml")
-
-for xml_file in xml_file_names:
-    print "xml file is ", xml_file
-    opts = options(lang = "C", output = "C/include_v"+protocol, \
-                   wire_protocol=protocol)
-    args = []
-    args.append(xml_file)
-    mavgen(opts, args)
-    xml_file_base = os.path.basename(xml_file)
-    xml_file_base = re.sub("\.xml","", xml_file_base)
-    print "xml_file_base is", xml_file_base
-    opts = options(lang = "python", \
-                   output="python/mavlink_"+xml_file_base+"_v"+protocol+".py", \
-                   wire_protocol=protocol)
-    mavgen(opts,args)
-
-mavlink_directory_list = ["common","matrixpilot"]
-for mavlink_directory in mavlink_directory_list :
-    # Look specifically for MatrixPilot directory structure
-    target_directory = "../../../../MAVLink/include/"+mavlink_directory
-    source_directory = "C/include_v"+protocol+"/"+mavlink_directory
-    if os.access(source_directory, os.R_OK):
-        if os.access(target_directory, os.W_OK):
-            print "Preparing to copy over files..."
-            print "About to remove all files in",target_directory
-            print "OK to continue ?[Yes / No]: ",
-            line = sys.stdin.readline()
-            if line == "Yes\n" or line == "yes\n" \
-               or line == "Y\n" or line == "y\n":
-                print "passed"
-                remove_include_files(target_directory)
-                copy_include_files(source_directory,target_directory)
-                print "Finished copying over include files"
-            else :
-                print "Your answer is No. Exiting Program"
-                sys.exit()
-        else :
-           print "Cannot find " + target_directory + "in MatrixPilot"
-           sys.exit()
-    else:
-        print "Could not find files to copy at", source_directory
-        print "Exiting Program."
-        sys.exit()
diff --git a/mavlink/share/pyshared/pymavlink/generator/gen_all.py b/mavlink/share/pyshared/pymavlink/generator/gen_all.py
deleted file mode 100644
index 5b24f85cb..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/gen_all.py
+++ /dev/null
@@ -1,44 +0,0 @@
-#!/usr/bin/env python
-
-'''
-Use mavgen.py on all available MAVLink XML definitions to generate
-C and Python MAVLink routines for sending and parsing the protocol
-
-Copyright Pete Hollands 2011
-Released under GNU GPL version 3 or later
-'''
-
-import os, sys, glob, re
-from mavgen import mavgen
-
-# allow import from the parent directory, where mavutil.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-class options:
-    """ a class to simulate the options of mavgen OptionsParser"""
-    def __init__(self, lang, output, wire_protocol):
-        self.language = lang
-        self.wire_protocol = wire_protocol
-        self.output = output
-
-protocols = [ '0.9', '1.0' ]
-
-for protocol in protocols :
-    xml_directory = './message_definitions/v'+protocol
-    print "xml_directory is", xml_directory
-    xml_file_names = glob.glob(xml_directory+'/*.xml')
-
-    for xml_file in xml_file_names:
-        print "xml file is ", xml_file
-        opts = options(lang = "C", output = "C/include_v"+protocol, \
-                       wire_protocol=protocol)
-        args = []
-        args.append(xml_file)
-        mavgen(opts, args)
-        xml_file_base = os.path.basename(xml_file)
-        xml_file_base = re.sub("\.xml","", xml_file_base)
-        print "xml_file_base is", xml_file_base
-        opts = options(lang = "python", \
-                       output="python/mavlink_"+xml_file_base+"_v"+protocol+".py", \
-                       wire_protocol=protocol)
-        mavgen(opts,args)
diff --git a/mavlink/share/pyshared/pymavlink/generator/gen_all.sh b/mavlink/share/pyshared/pymavlink/generator/gen_all.sh
deleted file mode 100644
index e8dafedc5..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/gen_all.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/sh
-
-for protocol in 0.9 1.0; do
- for xml in message_definitions/v$protocol/*.xml; do
-     base=$(basename $xml .xml)
-     ./mavgen.py --lang=C --wire-protocol=$protocol --output=C/include_v$protocol $xml || exit 1
-     ./mavgen.py --lang=python --wire-protocol=$protocol --output=python/mavlink_${base}_v$protocol.py $xml || exit 1
- done
-done
-
-cp -f python/mavlink_ardupilotmega_v0.9.py ../mavlink.py
-cp -f python/mavlink_ardupilotmega_v1.0.py ../mavlinkv10.py
diff --git a/mavlink/share/pyshared/pymavlink/generator/mavgen.py b/mavlink/share/pyshared/pymavlink/generator/mavgen.py
deleted file mode 100644
index 05f71f778..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/mavgen.py
+++ /dev/null
@@ -1,82 +0,0 @@
-#!/usr/bin/env python
-'''
-parse a MAVLink protocol XML file and generate a python implementation
-
-Copyright Andrew Tridgell 2011
-Released under GNU GPL version 3 or later
-'''
-
-def mavgen(opts, args) :
-    """Generate mavlink message formatters and parsers (C and Python ) using options
-    and args where args are a list of xml files. This function allows python
-    scripts under Windows to control mavgen using the same interface as
-    shell scripts under Unix"""
-    import sys, textwrap, os
-
-    import mavparse
-    import mavgen_python
-    import mavgen_c
-
-    xml = []
-
-    for fname in args:
-        print("Parsing %s" % fname)
-        xml.append(mavparse.MAVXML(fname, opts.wire_protocol))
-
-    # expand includes
-    for x in xml[:]:
-        for i in x.include:
-            fname = os.path.join(os.path.dirname(x.filename), i)
-            print("Parsing %s" % fname)
-            xml.append(mavparse.MAVXML(fname, opts.wire_protocol))
-
-            # include message lengths and CRCs too
-            for idx in range(0, 256):
-                if x.message_lengths[idx] == 0:
-                    x.message_lengths[idx] = xml[-1].message_lengths[idx]
-                    x.message_crcs[idx] = xml[-1].message_crcs[idx]
-                    x.message_names[idx] = xml[-1].message_names[idx]
-
-    # work out max payload size across all includes
-    largest_payload = 0
-    for x in xml:
-        if x.largest_payload > largest_payload:
-            largest_payload = x.largest_payload
-    for x in xml:
-        x.largest_payload = largest_payload
-
-    if mavparse.check_duplicates(xml):
-        sys.exit(1)
-
-    print("Found %u MAVLink message types in %u XML files" % (
-        mavparse.total_msgs(xml), len(xml)))
-
-    if opts.language == 'python':
-        mavgen_python.generate(opts.output, xml)
-    elif opts.language == 'C':
-        mavgen_c.generate(opts.output, xml)
-    else:
-        print("Unsupported language %s" % opts.language)
-    
-
-if __name__=="__main__":
-    import sys, textwrap, os
-
-    from optparse import OptionParser
-
-    # allow import from the parent directory, where mavutil.py is
-    sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-    import mavparse
-    import mavgen_python
-    import mavgen_c
-
-    parser = OptionParser("%prog [options] <XML files>")
-    parser.add_option("-o", "--output", dest="output", default="mavlink", help="output directory.")
-    parser.add_option("--lang", dest="language", default="python", help="language of generated code: 'Python' or 'C' [default: %default]")
-    parser.add_option("--wire-protocol", dest="wire_protocol", default=mavparse.PROTOCOL_0_9, help="MAVLink protocol version: '0.9' or '1.0'. [default: %default]")
-    (opts, args) = parser.parse_args()
-
-    if len(args) < 1:
-        parser.error("You must supply at least one MAVLink XML protocol definition")
-    mavgen(opts, args)
diff --git a/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py b/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py
deleted file mode 100644
index 255919f0d..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py
+++ /dev/null
@@ -1,581 +0,0 @@
-#!/usr/bin/env python
-'''
-parse a MAVLink protocol XML file and generate a C implementation
-
-Copyright Andrew Tridgell 2011
-Released under GNU GPL version 3 or later
-'''
-
-import sys, textwrap, os, time
-import mavparse, mavtemplate
-
-t = mavtemplate.MAVTemplate()
-
-def generate_version_h(directory, xml):
-    '''generate version.h'''
-    f = open(os.path.join(directory, "version.h"), mode='w')
-    t.write(f,'''
-/** @file
- *	@brief MAVLink comm protocol built from ${basename}.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
-
-#define MAVLINK_BUILD_DATE "${parse_time}"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "${wire_protocol_version}"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE ${largest_payload}
- 
-#endif // MAVLINK_VERSION_H
-''', xml)
-    f.close()
-
-def generate_mavlink_h(directory, xml):
-    '''generate mavlink.h'''
-    f = open(os.path.join(directory, "mavlink.h"), mode='w')
-    t.write(f,'''
-/** @file
- *	@brief MAVLink comm protocol built from ${basename}.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
-
-#ifndef MAVLINK_STX
-#define MAVLINK_STX ${protocol_marker}
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN ${mavlink_endian}
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS ${aligned_fields_define}
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA ${crc_extra_define}
-#endif
-
-#include "version.h"
-#include "${basename}.h"
-
-#endif // MAVLINK_H
-''', xml)
-    f.close()
-
-def generate_main_h(directory, xml):
-    '''generate main header per XML file'''
-    f = open(os.path.join(directory, xml.basename + ".h"), mode='w')
-    t.write(f, '''
-/** @file
- *	@brief MAVLink comm protocol generated from ${basename}.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef ${basename_upper}_H
-#define ${basename_upper}_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// MESSAGE LENGTHS AND CRCS
-
-#ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {${message_lengths_array}}
-#endif
-
-#ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {${message_crcs_array}}
-#endif
-
-#ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {${message_info_array}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_${basename_upper}
-
-${{include_list:#include "../${base}/${base}.h"
-}}
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION ${version}
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION ${version}
-#endif
-
-// ENUM DEFINITIONS
-
-${{enum:
-/** @brief ${description} */
-#ifndef HAVE_ENUM_${name}
-#define HAVE_ENUM_${name}
-enum ${name}
-{
-${{entry:	${name}=${value}, /* ${description} |${{param:${description}| }} */
-}}
-};
-#endif
-}}
-
-// MESSAGE DEFINITIONS
-${{message:#include "./mavlink_msg_${name_lower}.h"
-}}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // ${basename_upper}_H
-''', xml)
-
-    f.close()
-             
-
-def generate_message_h(directory, m):
-    '''generate per-message header for a XML file'''
-    f = open(os.path.join(directory, 'mavlink_msg_%s.h' % m.name_lower), mode='w')
-    t.write(f, '''
-// MESSAGE ${name} PACKING
-
-#define MAVLINK_MSG_ID_${name} ${id}
-
-typedef struct __mavlink_${name_lower}_t
-{
-${{ordered_fields: ${type} ${name}${array_suffix}; ///< ${description}
-}}
-} mavlink_${name_lower}_t;
-
-#define MAVLINK_MSG_ID_${name}_LEN ${wire_length}
-#define MAVLINK_MSG_ID_${id}_LEN ${wire_length}
-
-${{array_fields:#define MAVLINK_MSG_${msg_name}_FIELD_${name_upper}_LEN ${array_length}
-}}
-
-#define MAVLINK_MESSAGE_INFO_${name} { \\
-	"${name}", \\
-	${num_fields}, \\
-	{ ${{ordered_fields: { "${name}", ${c_print_format}, MAVLINK_TYPE_${type_upper}, ${array_length}, ${wire_offset}, offsetof(mavlink_${name_lower}_t, ${name}) }, \\
-        }} } \\
-}
-
-
-/**
- * @brief Pack a ${name_lower} 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
- *
-${{arg_fields: * @param ${name} ${description}
-}}
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_${name_lower}_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						      ${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[${wire_length}];
-${{scalar_fields:	_mav_put_${type}(buf, ${wire_offset}, ${putname});
-}}
-${{array_fields:	_mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
-}}
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, ${wire_length});
-#else
-	mavlink_${name_lower}_t packet;
-${{scalar_fields:	packet.${name} = ${putname};
-}}
-${{array_fields:	mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
-}}
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, ${wire_length});
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_${name};
-	return mavlink_finalize_message(msg, system_id, component_id, ${wire_length}${crc_extra_arg});
-}
-
-/**
- * @brief Pack a ${name_lower} 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 was sent over
- * @param msg The MAVLink message to compress the data into
-${{arg_fields: * @param ${name} ${description}
-}}
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_${name_lower}_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           ${{arg_fields:${array_const}${type} ${array_prefix}${name},}})
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[${wire_length}];
-${{scalar_fields:	_mav_put_${type}(buf, ${wire_offset}, ${putname});
-}}
-${{array_fields:	_mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
-}}
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, ${wire_length});
-#else
-	mavlink_${name_lower}_t packet;
-${{scalar_fields:	packet.${name} = ${putname};
-}}
-${{array_fields:	mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
-}}
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, ${wire_length});
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_${name};
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, ${wire_length}${crc_extra_arg});
-}
-
-/**
- * @brief Encode a ${name_lower} struct into a 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 ${name_lower} C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_${name_lower}_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_${name_lower}_t* ${name_lower})
-{
-	return mavlink_msg_${name_lower}_pack(system_id, component_id, msg,${{arg_fields: ${name_lower}->${name},}});
-}
-
-/**
- * @brief Send a ${name_lower} message
- * @param chan MAVLink channel to send the message
- *
-${{arg_fields: * @param ${name} ${description}
-}}
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_${name_lower}_send(mavlink_channel_t chan,${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[${wire_length}];
-${{scalar_fields:	_mav_put_${type}(buf, ${wire_offset}, ${putname});
-}}
-${{array_fields:	_mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
-}}
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, buf, ${wire_length}${crc_extra_arg});
-#else
-	mavlink_${name_lower}_t packet;
-${{scalar_fields:	packet.${name} = ${putname};
-}}
-${{array_fields:	mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
-}}
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, (const char *)&packet, ${wire_length}${crc_extra_arg});
-#endif
-}
-
-#endif
-
-// MESSAGE ${name} UNPACKING
-
-${{fields:
-/**
- * @brief Get field ${name} from ${name_lower} message
- *
- * @return ${description}
- */
-static inline ${return_type} mavlink_msg_${name_lower}_get_${name}(const mavlink_message_t* msg${get_arg})
-{
-	return _MAV_RETURN_${type}${array_tag}(msg, ${array_return_arg} ${wire_offset});
-}
-}}
-
-/**
- * @brief Decode a ${name_lower} message into a struct
- *
- * @param msg The message to decode
- * @param ${name_lower} C-struct to decode the message contents into
- */
-static inline void mavlink_msg_${name_lower}_decode(const mavlink_message_t* msg, mavlink_${name_lower}_t* ${name_lower})
-{
-#if MAVLINK_NEED_BYTE_SWAP
-${{ordered_fields:	${decode_left}mavlink_msg_${name_lower}_get_${name}(msg${decode_right});
-}}
-#else
-	memcpy(${name_lower}, _MAV_PAYLOAD(msg), ${wire_length});
-#endif
-}
-''', m)
-    f.close()
-
-
-def generate_testsuite_h(directory, xml):
-    '''generate testsuite.h per XML file'''
-    f = open(os.path.join(directory, "testsuite.h"), mode='w')
-    t.write(f, '''
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from ${basename}.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef ${basename_upper}_TESTSUITE_H
-#define ${basename_upper}_TESTSUITE_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-${{include_list:static void mavlink_test_${base}(uint8_t, uint8_t, mavlink_message_t *last_msg);
-}}
-static void mavlink_test_${basename}(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-${{include_list:	mavlink_test_${base}(system_id, component_id, last_msg);
-}}
-	mavlink_test_${basename}(system_id, component_id, last_msg);
-}
-#endif
-
-${{include_list:#include "../${base}/testsuite.h"
-}}
-
-${{message:
-static void mavlink_test_${name_lower}(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_${name_lower}_t packet_in = {
-		${{ordered_fields:${c_test_value},
-	}}};
-	mavlink_${name_lower}_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        ${{scalar_fields:	packet1.${name} = packet_in.${name};
-        }}
-        ${{array_fields:	mav_array_memcpy(packet1.${name}, packet_in.${name}, sizeof(${type})*${array_length});
-        }}
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_${name_lower}_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_${name_lower}_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_${name_lower}_pack(system_id, component_id, &msg ${{arg_fields:, packet1.${name} }});
-	mavlink_msg_${name_lower}_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_${name_lower}_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg ${{arg_fields:, packet1.${name} }});
-	mavlink_msg_${name_lower}_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_${name_lower}_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_${name_lower}_send(MAVLINK_COMM_1 ${{arg_fields:, packet1.${name} }});
-	mavlink_msg_${name_lower}_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-}}
-
-static void mavlink_test_${basename}(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-${{message:	mavlink_test_${name_lower}(system_id, component_id, last_msg);
-}}
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // ${basename_upper}_TESTSUITE_H
-''', xml)
-
-    f.close()
-
-def copy_fixed_headers(directory, xml):
-    '''copy the fixed protocol headers to the target directory'''
-    import shutil
-    hlist = [ 'protocol.h', 'mavlink_helpers.h', 'mavlink_types.h', 'checksum.h', 'mavlink_protobuf_manager.hpp' ]
-    basepath = os.path.dirname(os.path.realpath(__file__))
-    srcpath = os.path.join(basepath, 'C/include_v%s' % xml.wire_protocol_version)
-    print("Copying fixed headers")
-    for h in hlist:
-        if (not (h == 'mavlink_protobuf_manager.hpp' and xml.wire_protocol_version == '0.9')):
-           src = os.path.realpath(os.path.join(srcpath, h))
-           dest = os.path.realpath(os.path.join(directory, h))
-           if src == dest:
-               continue
-           shutil.copy(src, dest)
-    # XXX This is a hack - to be removed
-    if (xml.basename == 'pixhawk' and xml.wire_protocol_version == '1.0'):
-        h = 'pixhawk/pixhawk.pb.h'
-        src = os.path.realpath(os.path.join(srcpath, h))
-        dest = os.path.realpath(os.path.join(directory, h))
-        shutil.copy(src, dest)
-        
-def copy_fixed_sources(directory, xml):
-    # XXX This is a hack - to be removed
-    import shutil
-    basepath = os.path.dirname(os.path.realpath(__file__))
-    srcpath = os.path.join(basepath, 'C/src_v%s' % xml.wire_protocol_version)
-    if (xml.basename == 'pixhawk' and xml.wire_protocol_version == '1.0'):
-        print("Copying fixed sources")
-        src = os.path.realpath(os.path.join(srcpath, 'pixhawk/pixhawk.pb.cc'))
-        dest = os.path.realpath(os.path.join(directory, '../../../share/mavlink/src/v%s/pixhawk/pixhawk.pb.cc' % xml.wire_protocol_version))
-        destdir = os.path.realpath(os.path.join(directory, '../../../share/mavlink/src/v%s/pixhawk' % xml.wire_protocol_version))
-        try:
-           os.makedirs(destdir)
-        except:
-           print("Not re-creating directory")
-        shutil.copy(src, dest)
-        print("Copied to"),
-        print(dest)
-
-class mav_include(object):
-    def __init__(self, base):
-        self.base = base
-
-def generate_one(basename, xml):
-    '''generate headers for one XML file'''
-
-    directory = os.path.join(basename, xml.basename)
-
-    print("Generating C implementation in directory %s" % directory)
-    mavparse.mkdir_p(directory)
-
-    if xml.little_endian:
-        xml.mavlink_endian = "MAVLINK_LITTLE_ENDIAN"
-    else:
-        xml.mavlink_endian = "MAVLINK_BIG_ENDIAN"
-
-    if xml.crc_extra:
-        xml.crc_extra_define = "1"
-    else:
-        xml.crc_extra_define = "0"
-
-    if xml.sort_fields:
-        xml.aligned_fields_define = "1"
-    else:
-        xml.aligned_fields_define = "0"
-
-    # work out the included headers
-    xml.include_list = []
-    for i in xml.include:
-        base = i[:-4]
-        xml.include_list.append(mav_include(base))
-
-    # form message lengths array
-    xml.message_lengths_array = ''
-    for mlen in xml.message_lengths:
-        xml.message_lengths_array += '%u, ' % mlen
-    xml.message_lengths_array = xml.message_lengths_array[:-2]
-
-    # and message CRCs array
-    xml.message_crcs_array = ''
-    for crc in xml.message_crcs:
-        xml.message_crcs_array += '%u, ' % crc
-    xml.message_crcs_array = xml.message_crcs_array[:-2]
-
-    # form message info array
-    xml.message_info_array = ''
-    for name in xml.message_names:
-        if name is not None:
-            xml.message_info_array += 'MAVLINK_MESSAGE_INFO_%s, ' % name
-        else:
-            # Several C compilers don't accept {NULL} for
-            # multi-dimensional arrays and structs
-            # feed the compiler a "filled" empty message
-            xml.message_info_array += '{"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, '
-    xml.message_info_array = xml.message_info_array[:-2]
-
-    # add some extra field attributes for convenience with arrays
-    for m in xml.message:
-        m.msg_name = m.name
-        if xml.crc_extra:
-            m.crc_extra_arg = ", %s" % m.crc_extra
-        else:
-            m.crc_extra_arg = ""
-        for f in m.fields:
-            if f.print_format is None:
-                f.c_print_format = 'NULL'
-            else:
-                f.c_print_format = '"%s"' % f.print_format
-            if f.array_length != 0:
-                f.array_suffix = '[%u]' % f.array_length
-                f.array_prefix = '*'
-                f.array_tag = '_array'
-                f.array_arg = ', %u' % f.array_length
-                f.array_return_arg = '%s, %u, ' % (f.name, f.array_length)
-                f.array_const = 'const '
-                f.decode_left = ''
-                f.decode_right = ', %s->%s' % (m.name_lower, f.name)
-                f.return_type = 'uint16_t'
-                f.get_arg = ', %s *%s' % (f.type, f.name)
-                if f.type == 'char':
-                    f.c_test_value = '"%s"' % f.test_value
-                else:
-                    test_strings = []
-                    for v in f.test_value:
-                        test_strings.append(str(v))
-                    f.c_test_value = '{ %s }' % ', '.join(test_strings)
-            else:
-                f.array_suffix = ''
-                f.array_prefix = ''
-                f.array_tag = ''
-                f.array_arg = ''
-                f.array_return_arg = ''
-                f.array_const = ''
-                f.decode_left = "%s->%s = " % (m.name_lower, f.name)
-                f.decode_right = ''
-                f.get_arg = ''
-                f.return_type = f.type
-                if f.type == 'char':
-                    f.c_test_value = "'%s'" % f.test_value
-                elif f.type == 'uint64_t':
-                    f.c_test_value = "%sULL" % f.test_value                    
-                elif f.type == 'int64_t':
-                    f.c_test_value = "%sLL" % f.test_value                    
-                else:
-                    f.c_test_value = f.test_value
-
-    # cope with uint8_t_mavlink_version
-    for m in xml.message:
-        m.arg_fields = []
-        m.array_fields = []
-        m.scalar_fields = []
-        for f in m.ordered_fields:
-            if f.array_length != 0:
-                m.array_fields.append(f)
-            else:
-                m.scalar_fields.append(f)
-        for f in m.fields:
-            if not f.omit_arg:
-                m.arg_fields.append(f)
-                f.putname = f.name
-            else:
-                f.putname = f.const_value
-
-    generate_mavlink_h(directory, xml)
-    generate_version_h(directory, xml)
-    generate_main_h(directory, xml)
-    for m in xml.message:
-        generate_message_h(directory, m)
-    generate_testsuite_h(directory, xml)
-
-
-def generate(basename, xml_list):
-    '''generate complete MAVLink C implemenation'''
-
-    for xml in xml_list:
-        generate_one(basename, xml)
-    copy_fixed_headers(basename, xml_list[0])
-    copy_fixed_sources(basename, xml_list[0])
diff --git a/mavlink/share/pyshared/pymavlink/generator/mavgen_python.py b/mavlink/share/pyshared/pymavlink/generator/mavgen_python.py
deleted file mode 100644
index fad366a68..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/mavgen_python.py
+++ /dev/null
@@ -1,455 +0,0 @@
-#!/usr/bin/env python
-'''
-parse a MAVLink protocol XML file and generate a python implementation
-
-Copyright Andrew Tridgell 2011
-Released under GNU GPL version 3 or later
-'''
-
-import sys, textwrap, os
-import mavparse, mavtemplate
-
-t = mavtemplate.MAVTemplate()
-
-def generate_preamble(outf, msgs, args, xml):
-    print("Generating preamble")
-    t.write(outf, """
-'''
-MAVLink protocol implementation (auto-generated by mavgen.py)
-
-Generated from: ${FILELIST}
-
-Note: this file has been auto-generated. DO NOT EDIT
-'''
-
-import struct, array, mavutil, time
-
-WIRE_PROTOCOL_VERSION = "${WIRE_PROTOCOL_VERSION}"
-
-class MAVLink_header(object):
-    '''MAVLink message header'''
-    def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
-        self.mlen = mlen
-        self.seq = seq
-        self.srcSystem = srcSystem
-        self.srcComponent = srcComponent
-        self.msgId = msgId
-
-    def pack(self):
-        return struct.pack('BBBBBB', ${PROTOCOL_MARKER}, self.mlen, self.seq,
-                          self.srcSystem, self.srcComponent, self.msgId)
-
-class MAVLink_message(object):
-    '''base MAVLink message class'''
-    def __init__(self, msgId, name):
-        self._header     = MAVLink_header(msgId)
-        self._payload    = None
-        self._msgbuf     = None
-        self._crc        = None
-        self._fieldnames = []
-        self._type       = name
-
-    def get_msgbuf(self):
-        return self._msgbuf
-
-    def get_header(self):
-        return self._header
-
-    def get_payload(self):
-        return self._payload
-
-    def get_crc(self):
-        return self._crc
-
-    def get_fieldnames(self):
-        return self._fieldnames
-
-    def get_type(self):
-        return self._type
-
-    def get_msgId(self):
-        return self._header.msgId
-
-    def get_srcSystem(self):
-        return self._header.srcSystem
-
-    def get_srcComponent(self):
-        return self._header.srcComponent
-
-    def get_seq(self):
-        return self._header.seq
-
-    def __str__(self):
-        ret = '%s {' % self._type
-        for a in self._fieldnames:
-            v = getattr(self, a)
-            ret += '%s : %s, ' % (a, v)
-        ret = ret[0:-2] + '}'
-        return ret            
-
-    def pack(self, mav, crc_extra, payload):
-        self._payload = payload
-        self._header  = MAVLink_header(self._header.msgId, len(payload), mav.seq,
-                                       mav.srcSystem, mav.srcComponent)
-        self._msgbuf = self._header.pack() + payload
-        crc = mavutil.x25crc(self._msgbuf[1:])
-        if ${crc_extra}: # using CRC extra
-            crc.accumulate(chr(crc_extra))
-        self._crc = crc.crc
-        self._msgbuf += struct.pack('<H', self._crc)
-        return self._msgbuf
-
-""", {'FILELIST' : ",".join(args),
-      'PROTOCOL_MARKER' : xml.protocol_marker,
-      'crc_extra' : xml.crc_extra,
-      'WIRE_PROTOCOL_VERSION' : xml.wire_protocol_version })
-
-
-def generate_enums(outf, enums):
-    print("Generating enums")
-    outf.write("\n# enums\n")
-    wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent="                        # ")
-    for e in enums:
-        outf.write("\n# %s\n" % e.name)
-        for entry in e.entry:
-            outf.write("%s = %u # %s\n" % (entry.name, entry.value, wrapper.fill(entry.description)))
-
-def generate_message_ids(outf, msgs):
-    print("Generating message IDs")
-    outf.write("\n# message IDs\n")
-    outf.write("MAVLINK_MSG_ID_BAD_DATA = -1\n")
-    for m in msgs:
-        outf.write("MAVLINK_MSG_ID_%s = %u\n" % (m.name.upper(), m.id))
-
-def generate_classes(outf, msgs):
-    print("Generating class definitions")
-    wrapper = textwrap.TextWrapper(initial_indent="        ", subsequent_indent="        ")
-    for m in msgs:
-        outf.write("""
-class MAVLink_%s_message(MAVLink_message):
-        '''
-%s
-        '''
-        def __init__(self""" % (m.name.lower(), wrapper.fill(m.description.strip())))
-        if len(m.fields) != 0:
-                outf.write(", " + ", ".join(m.fieldnames))
-        outf.write("):\n")
-        outf.write("                MAVLink_message.__init__(self, MAVLINK_MSG_ID_%s, '%s')\n" % (m.name.upper(), m.name.upper()))
-        if len(m.fieldnames) != 0:
-                outf.write("                self._fieldnames = ['%s']\n" % "', '".join(m.fieldnames))
-        for f in m.fields:
-                outf.write("                self.%s = %s\n" % (f.name, f.name))
-        outf.write("""
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, %u, struct.pack('%s'""" % (m.crc_extra, m.fmtstr))
-        if len(m.fields) != 0:
-                outf.write(", self." + ", self.".join(m.ordered_fieldnames))
-        outf.write("))\n")
-
-
-def mavfmt(field):
-    '''work out the struct format for a type'''
-    map = {
-        'float'    : 'f',
-        'double'   : 'd',
-        'char'     : 'c',
-        'int8_t'   : 'b',
-        'uint8_t'  : 'B',
-        'uint8_t_mavlink_version'  : 'B',
-        'int16_t'  : 'h',
-        'uint16_t' : 'H',
-        'int32_t'  : 'i',
-        'uint32_t' : 'I',
-        'int64_t'  : 'q',
-        'uint64_t' : 'Q',
-        }
-
-    if field.array_length:
-        if field.type in ['char', 'int8_t', 'uint8_t']:
-            return str(field.array_length)+'s'
-        return str(field.array_length)+map[field.type]
-    return map[field.type]
-
-def generate_mavlink_class(outf, msgs, xml):
-    print("Generating MAVLink class")
-
-    outf.write("\n\nmavlink_map = {\n");
-    for m in msgs:
-        outf.write("        MAVLINK_MSG_ID_%s : ( '%s', MAVLink_%s_message, %s, %u ),\n" % (
-            m.name.upper(), m.fmtstr, m.name.lower(), m.order_map, m.crc_extra))
-    outf.write("}\n\n")
-    
-    t.write(outf, """
-class MAVError(Exception):
-        '''MAVLink error class'''
-        def __init__(self, msg):
-            Exception.__init__(self, msg)
-            self.message = msg
-
-class MAVString(str):
-        '''NUL terminated string'''
-        def __init__(self, s):
-                str.__init__(self)
-        def __str__(self):
-            i = self.find(chr(0))
-            if i == -1:
-                return self[:]
-            return self[0:i]
-
-class MAVLink_bad_data(MAVLink_message):
-        '''
-        a piece of bad data in a mavlink stream
-        '''
-        def __init__(self, data, reason):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_BAD_DATA, 'BAD_DATA')
-                self._fieldnames = ['data', 'reason']
-                self.data = data
-                self.reason = reason
-                self._msgbuf = data
-            
-class MAVLink(object):
-        '''MAVLink protocol handling class'''
-        def __init__(self, file, srcSystem=0, srcComponent=0):
-                self.seq = 0
-                self.file = file
-                self.srcSystem = srcSystem
-                self.srcComponent = srcComponent
-                self.callback = None
-                self.callback_args = None
-                self.callback_kwargs = None
-                self.buf = array.array('B')
-                self.expected_length = 6
-                self.have_prefix_error = False
-                self.robust_parsing = False
-                self.protocol_marker = ${protocol_marker}
-                self.little_endian = ${little_endian}
-                self.crc_extra = ${crc_extra}
-                self.sort_fields = ${sort_fields}
-                self.total_packets_sent = 0
-                self.total_bytes_sent = 0
-                self.total_packets_received = 0
-                self.total_bytes_received = 0
-                self.total_receive_errors = 0
-                self.startup_time = time.time()
-
-        def set_callback(self, callback, *args, **kwargs):
-            self.callback = callback
-            self.callback_args = args
-            self.callback_kwargs = kwargs
-            
-        def send(self, mavmsg):
-                '''send a MAVLink message'''
-                buf = mavmsg.pack(self)
-                self.file.write(buf)
-                self.seq = (self.seq + 1) % 255
-                self.total_packets_sent += 1
-                self.total_bytes_sent += len(buf)
-
-        def bytes_needed(self):
-            '''return number of bytes needed for next parsing stage'''
-            ret = self.expected_length - len(self.buf)
-            if ret <= 0:
-                return 1
-            return ret
-
-        def parse_char(self, c):
-            '''input some data bytes, possibly returning a new message'''
-            if isinstance(c, str):
-                self.buf.fromstring(c)
-            else:
-                self.buf.extend(c)
-            self.total_bytes_received += len(c)
-            if len(self.buf) >= 1 and self.buf[0] != ${protocol_marker}:
-                magic = self.buf[0]
-                self.buf = self.buf[1:]
-                if self.robust_parsing:
-                    m = MAVLink_bad_data(chr(magic), "Bad prefix")
-                    if self.callback:
-                        self.callback(m, *self.callback_args, **self.callback_kwargs)
-                    self.expected_length = 6
-                    self.total_receive_errors += 1
-                    return m
-                if self.have_prefix_error:
-                    return None
-                self.have_prefix_error = True
-                self.total_receive_errors += 1
-                raise MAVError("invalid MAVLink prefix '%s'" % magic) 
-            self.have_prefix_error = False
-            if len(self.buf) >= 2:
-                (magic, self.expected_length) = struct.unpack('BB', self.buf[0:2])
-                self.expected_length += 8
-            if self.expected_length >= 8 and len(self.buf) >= self.expected_length:
-                mbuf = self.buf[0:self.expected_length]
-                self.buf = self.buf[self.expected_length:]
-                self.expected_length = 6
-                if self.robust_parsing:
-                    try:
-                        m = self.decode(mbuf)
-                        self.total_packets_received += 1
-                    except MAVError as reason:
-                        m = MAVLink_bad_data(mbuf, reason.message)
-                        self.total_receive_errors += 1
-                else:
-                    m = self.decode(mbuf)
-                    self.total_packets_received += 1
-                if self.callback:
-                    self.callback(m, *self.callback_args, **self.callback_kwargs)
-                return m
-            return None
-
-        def parse_buffer(self, s):
-            '''input some data bytes, possibly returning a list of new messages'''
-            m = self.parse_char(s)
-            if m is None:
-                return None
-            ret = [m]
-            while True:
-                m = self.parse_char("")
-                if m is None:
-                    return ret
-                ret.append(m)
-            return ret
-
-        def decode(self, msgbuf):
-                '''decode a buffer as a MAVLink message'''
-                # decode the header
-                try:
-                    magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
-                if ord(magic) != ${protocol_marker}:
-                    raise MAVError("invalid MAVLink prefix '%s'" % magic)
-                if mlen != len(msgbuf)-8:
-                    raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf)-8, mlen, msgId))
-
-                if not msgId in mavlink_map:
-                    raise MAVError('unknown MAVLink message ID %u' % msgId)
-
-                # decode the payload
-                (fmt, type, order_map, crc_extra) = mavlink_map[msgId]
-
-                # decode the checksum
-                try:
-                    crc, = struct.unpack('<H', msgbuf[-2:])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink CRC: %s' % emsg)
-                crc2 = mavutil.x25crc(msgbuf[1:-2])
-                if ${crc_extra}: # using CRC extra 
-                    crc2.accumulate(chr(crc_extra))
-                if crc != crc2.crc:
-                    raise MAVError('invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' % (msgId, crc, crc2.crc))
-
-                try:
-                    t = struct.unpack(fmt, msgbuf[6:-2])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink payload type=%s fmt=%s payloadLength=%u: %s' % (
-                        type, fmt, len(msgbuf[6:-2]), emsg))
-
-                tlist = list(t)
-                # handle sorted fields
-                if ${sort_fields}:
-                    t = tlist[:]
-                    for i in range(0, len(tlist)):
-                        tlist[i] = t[order_map[i]]
-
-                # terminate any strings
-                for i in range(0, len(tlist)):
-                    if isinstance(tlist[i], str):
-                        tlist[i] = MAVString(tlist[i])
-                t = tuple(tlist)
-                # construct the message object
-                try:
-                    m = type(*t)
-                except Exception as emsg:
-                    raise MAVError('Unable to instantiate MAVLink message of type %s : %s' % (type, emsg))
-                m._msgbuf = msgbuf
-                m._payload = msgbuf[6:-2]
-                m._crc = crc
-                m._header = MAVLink_header(msgId, mlen, seq, srcSystem, srcComponent)
-                return m
-""", xml)
-
-def generate_methods(outf, msgs):
-    print("Generating methods")
-
-    def field_descriptions(fields):
-        ret = ""
-        for f in fields:
-            ret += "                %-18s        : %s (%s)\n" % (f.name, f.description.strip(), f.type)
-        return ret
-
-    wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent="                ")
-
-    for m in msgs:
-        comment = "%s\n\n%s" % (wrapper.fill(m.description.strip()), field_descriptions(m.fields))
-
-        selffieldnames = 'self, '
-        for f in m.fields:
-            if f.omit_arg:
-                selffieldnames += '%s=%s, ' % (f.name, f.const_value)
-            else:
-                selffieldnames += '%s, ' % f.name
-        selffieldnames = selffieldnames[:-2]
-
-        sub = {'NAMELOWER'      : m.name.lower(),
-               'SELFFIELDNAMES' : selffieldnames,
-               'COMMENT'        : comment,
-               'FIELDNAMES'     : ", ".join(m.fieldnames)}
-
-        t.write(outf, """
-        def ${NAMELOWER}_encode(${SELFFIELDNAMES}):
-                '''
-                ${COMMENT}
-                '''
-                msg = MAVLink_${NAMELOWER}_message(${FIELDNAMES})
-                msg.pack(self)
-                return msg
-            
-""", sub)
-
-        t.write(outf, """
-        def ${NAMELOWER}_send(${SELFFIELDNAMES}):
-                '''
-                ${COMMENT}
-                '''
-                return self.send(self.${NAMELOWER}_encode(${FIELDNAMES}))
-            
-""", sub)
-
-
-def generate(basename, xml):
-    '''generate complete python implemenation'''
-    if basename.endswith('.py'):
-        filename = basename
-    else:
-        filename = basename + '.py'
-
-    msgs = []
-    enums = []
-    filelist = []
-    for x in xml:
-        msgs.extend(x.message)
-        enums.extend(x.enum)
-        filelist.append(os.path.basename(x.filename))
-
-    for m in msgs:
-        if xml[0].little_endian:
-            m.fmtstr = '<'
-        else:
-            m.fmtstr = '>'
-        for f in m.ordered_fields:
-            m.fmtstr += mavfmt(f)
-        m.order_map = [ 0 ] * len(m.fieldnames)
-        for i in range(0, len(m.fieldnames)):
-            m.order_map[i] = m.ordered_fieldnames.index(m.fieldnames[i])
-
-    print("Generating %s" % filename)
-    outf = open(filename, "w")
-    generate_preamble(outf, msgs, filelist, xml[0])
-    generate_enums(outf, enums)
-    generate_message_ids(outf, msgs)
-    generate_classes(outf, msgs)
-    generate_mavlink_class(outf, msgs, xml[0])
-    generate_methods(outf, msgs)
-    outf.close()
-    print("Generated %s OK" % filename)
diff --git a/mavlink/share/pyshared/pymavlink/generator/mavparse.py b/mavlink/share/pyshared/pymavlink/generator/mavparse.py
deleted file mode 100644
index cd2e6a55f..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/mavparse.py
+++ /dev/null
@@ -1,372 +0,0 @@
-#!/usr/bin/env python
-'''
-mavlink python parse functions
-
-Copyright Andrew Tridgell 2011
-Released under GNU GPL version 3 or later
-'''
-
-import xml.parsers.expat, os, errno, time, sys, operator, mavutil
-
-PROTOCOL_0_9 = "0.9"
-PROTOCOL_1_0 = "1.0"
-
-class MAVParseError(Exception):
-    def __init__(self, message, inner_exception=None):
-        self.message = message
-        self.inner_exception = inner_exception
-        self.exception_info = sys.exc_info()
-    def __str__(self):
-        return self.message
-
-class MAVField(object):
-    def __init__(self, name, type, print_format, xml, description=''):
-        self.name = name
-        self.name_upper = name.upper()
-        self.description = description
-        self.array_length = 0
-        self.omit_arg = False
-        self.const_value = None
-        self.print_format = print_format
-        lengths = {
-        'float'    : 4,
-        'double'   : 8,
-        'char'     : 1,
-        'int8_t'   : 1,
-        'uint8_t'  : 1,
-        'uint8_t_mavlink_version'  : 1,
-        'int16_t'  : 2,
-        'uint16_t' : 2,
-        'int32_t'  : 4,
-        'uint32_t' : 4,
-        'int64_t'  : 8,
-        'uint64_t' : 8,
-        }
-
-        if type=='uint8_t_mavlink_version':
-            type = 'uint8_t'
-            self.omit_arg = True
-            self.const_value = xml.version
-
-        aidx = type.find("[")
-        if aidx != -1:
-            assert type[-1:] == ']'
-            self.array_length = int(type[aidx+1:-1])
-            type = type[0:aidx]
-            if type == 'array':
-                type = 'int8_t'
-        if type in lengths:
-            self.type_length = lengths[type]
-            self.type = type
-        elif (type+"_t") in lengths:
-            self.type_length = lengths[type+"_t"]
-            self.type = type+'_t'
-        else:
-            raise MAVParseError("unknown type '%s'" % type)
-        if self.array_length != 0:
-            self.wire_length = self.array_length * self.type_length
-        else:
-            self.wire_length = self.type_length
-        self.type_upper = self.type.upper()
-
-    def gen_test_value(self, i):
-        '''generate a testsuite value for a MAVField'''
-        if self.const_value:
-            return self.const_value
-        elif self.type == 'float':
-            return 17.0 + self.wire_offset*7 + i
-        elif self.type == 'double':
-            return 123.0 + self.wire_offset*7 + i
-        elif self.type == 'char':
-            return chr(ord('A') + (self.wire_offset + i)%26)
-        elif self.type in [ 'int8_t', 'uint8_t' ]:
-            return (5 + self.wire_offset*67 + i) & 0xFF
-        elif self.type in ['int16_t', 'uint16_t']:
-            return (17235 + self.wire_offset*52 + i) & 0xFFFF
-        elif self.type in ['int32_t', 'uint32_t']:
-            return (963497464 + self.wire_offset*52 + i)&0xFFFFFFFF
-        elif self.type in ['int64_t', 'uint64_t']:
-            return 93372036854775807 + self.wire_offset*63 + i
-        else:
-            raise MAVError('unknown type %s' % self.type)
-
-    def set_test_value(self):
-        '''set a testsuite value for a MAVField'''
-        if self.array_length:
-            self.test_value = []
-            for i in range(self.array_length):
-                self.test_value.append(self.gen_test_value(i))
-        else:
-                self.test_value = self.gen_test_value(0)
-        if self.type == 'char' and self.array_length:
-            v = ""
-            for c in self.test_value:
-                v += c
-            self.test_value = v[:-1]
-
-
-class MAVType(object):
-    def __init__(self, name, id, linenumber, description=''):
-        self.name = name
-        self.name_lower = name.lower()
-        self.linenumber = linenumber
-        self.id = int(id)
-        self.description = description
-        self.fields = []
-        self.fieldnames = []
-
-class MAVEnumParam(object):
-    def __init__(self, index, description=''):
-        self.index = index
-        self.description = description
-
-class MAVEnumEntry(object):
-    def __init__(self, name, value, description='', end_marker=False):
-        self.name = name
-        self.value = value
-        self.description = description
-        self.param = []
-        self.end_marker = end_marker
-
-class MAVEnum(object):
-    def __init__(self, name, linenumber, description=''):
-        self.name = name
-        self.description = description
-        self.entry = []
-        self.highest_value = 0
-        self.linenumber = linenumber
-
-class MAVXML(object):
-    '''parse a mavlink XML file'''
-    def __init__(self, filename, wire_protocol_version=PROTOCOL_0_9):
-        self.filename = filename
-        self.basename = os.path.basename(filename)
-        if self.basename.lower().endswith(".xml"):
-            self.basename = self.basename[:-4]
-        self.basename_upper = self.basename.upper()
-        self.message = []
-        self.enum = []
-        self.parse_time = time.asctime()
-        self.version = 2
-        self.include = []
-        self.wire_protocol_version = wire_protocol_version
-
-        if wire_protocol_version == PROTOCOL_0_9:
-            self.protocol_marker = ord('U')
-            self.sort_fields = False
-            self.little_endian = False
-            self.crc_extra = False
-        elif wire_protocol_version == PROTOCOL_1_0:
-            self.protocol_marker = 0xFE
-            self.sort_fields = True
-            self.little_endian = True
-            self.crc_extra = True
-        else:
-            print("Unknown wire protocol version")
-            print("Available versions are: %s %s" % (PROTOCOL_0_9, PROTOCOL_1_0))
-            raise MAVParseError('Unknown MAVLink wire protocol version %s' % wire_protocol_version)
-
-        in_element_list = []
-
-        def check_attrs(attrs, check, where):
-            for c in check:
-                if not c in attrs:
-                    raise MAVParseError('expected missing %s "%s" attribute at %s:%u' % (
-                        where, c, filename, p.CurrentLineNumber))
-
-        def start_element(name, attrs):
-            in_element_list.append(name)
-            in_element = '.'.join(in_element_list)
-            #print in_element
-            if in_element == "mavlink.messages.message":
-                check_attrs(attrs, ['name', 'id'], 'message')
-                self.message.append(MAVType(attrs['name'], attrs['id'], p.CurrentLineNumber))
-            elif in_element == "mavlink.messages.message.field":
-                check_attrs(attrs, ['name', 'type'], 'field')
-                if 'print_format' in attrs:
-                    print_format = attrs['print_format']
-                else:
-                    print_format = None
-                self.message[-1].fields.append(MAVField(attrs['name'], attrs['type'],
-                                                        print_format, self))
-            elif in_element == "mavlink.enums.enum":
-                check_attrs(attrs, ['name'], 'enum')
-                self.enum.append(MAVEnum(attrs['name'], p.CurrentLineNumber))
-            elif in_element == "mavlink.enums.enum.entry":
-                check_attrs(attrs, ['name'], 'enum entry')
-                if 'value' in attrs:
-                    value = int(attrs['value'])
-                else:
-                    value = self.enum[-1].highest_value + 1
-                if (value > self.enum[-1].highest_value):
-                    self.enum[-1].highest_value = value
-                self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value))
-            elif in_element == "mavlink.enums.enum.entry.param":
-                check_attrs(attrs, ['index'], 'enum param')
-                self.enum[-1].entry[-1].param.append(MAVEnumParam(attrs['index']))
-
-        def end_element(name):
-            in_element = '.'.join(in_element_list)
-            if in_element == "mavlink.enums.enum":
-                # add a ENUM_END
-                self.enum[-1].entry.append(MAVEnumEntry("%s_ENUM_END" % self.enum[-1].name,
-                                                        self.enum[-1].highest_value+1, end_marker=True))
-            in_element_list.pop()
-
-        def char_data(data):
-            in_element = '.'.join(in_element_list)
-            if in_element == "mavlink.messages.message.description":
-                self.message[-1].description += data
-            elif in_element == "mavlink.messages.message.field":
-                self.message[-1].fields[-1].description += data
-            elif in_element == "mavlink.enums.enum.description":
-                self.enum[-1].description += data
-            elif in_element == "mavlink.enums.enum.entry.description":
-                self.enum[-1].entry[-1].description += data
-            elif in_element == "mavlink.enums.enum.entry.param":
-                self.enum[-1].entry[-1].param[-1].description += data
-            elif in_element == "mavlink.version":
-                self.version = int(data)
-            elif in_element == "mavlink.include":
-                self.include.append(data)
-
-        f = open(filename, mode='rb')
-        p = xml.parsers.expat.ParserCreate()
-        p.StartElementHandler = start_element
-        p.EndElementHandler = end_element
-        p.CharacterDataHandler = char_data
-        p.ParseFile(f)
-        f.close()
-
-        self.message_lengths = [ 0 ] * 256
-        self.message_crcs = [ 0 ] * 256
-        self.message_names = [ None ] * 256
-        self.largest_payload = 0
-
-        for m in self.message:
-            m.wire_length = 0
-            m.fieldnames = []
-            m.ordered_fieldnames = []
-            if self.sort_fields:
-                m.ordered_fields = sorted(m.fields,
-                                          key=operator.attrgetter('type_length'),
-                                          reverse=True)
-            else:
-                m.ordered_fields = m.fields
-            for f in m.fields:
-                m.fieldnames.append(f.name)
-            for f in m.ordered_fields:
-                f.wire_offset = m.wire_length
-                m.wire_length += f.wire_length
-                m.ordered_fieldnames.append(f.name)
-                f.set_test_value()
-            m.num_fields = len(m.fieldnames)
-            if m.num_fields > 64:
-                raise MAVParseError("num_fields=%u : Maximum number of field names allowed is" % (
-                    m.num_fields, 64))
-            m.crc_extra = message_checksum(m)
-            self.message_lengths[m.id] = m.wire_length
-            self.message_names[m.id] = m.name
-            self.message_crcs[m.id] = m.crc_extra
-            if m.wire_length > self.largest_payload:
-                self.largest_payload = m.wire_length
-
-            if m.wire_length+8 > 64:
-                print("Note: message %s is longer than 64 bytes long (%u bytes), which can cause fragmentation since many radio modems use 64 bytes as maximum air transfer unit." % (m.name, m.wire_length+8))
-
-    def __str__(self):
-        return "MAVXML for %s from %s (%u message, %u enums)" % (
-            self.basename, self.filename, len(self.message), len(self.enum))
-    
-
-def message_checksum(msg):
-    '''calculate a 8-bit checksum of the key fields of a message, so we
-       can detect incompatible XML changes'''
-    crc = mavutil.x25crc(msg.name + ' ')
-    for f in msg.ordered_fields:
-        crc.accumulate(f.type + ' ')
-        crc.accumulate(f.name + ' ')
-        if f.array_length:
-            crc.accumulate(chr(f.array_length))
-    return (crc.crc&0xFF) ^ (crc.crc>>8)
-
-def merge_enums(xml):
-    '''merge enums between XML files'''
-    emap = {}
-    for x in xml:
-        newenums = []
-        for enum in x.enum:
-            if enum.name in emap:
-                emap[enum.name].entry.pop() # remove end marker
-                emap[enum.name].entry.extend(enum.entry)
-                print("Merged enum %s" % enum.name)
-            else:
-                newenums.append(enum)
-                emap[enum.name] = enum
-        x.enum = newenums
-    # sort by value
-    for e in emap:
-        emap[e].entry = sorted(emap[e].entry,
-                               key=operator.attrgetter('value'),
-                               reverse=False)
-
-
-def check_duplicates(xml):
-    '''check for duplicate message IDs'''
-
-    merge_enums(xml)
-
-    msgmap = {}
-    enummap = {}
-    for x in xml:
-        for m in x.message:
-            if m.id in msgmap:
-                print("ERROR: Duplicate message id %u for %s (%s:%u) also used by %s" % (
-                    m.id, m.name,
-                    x.filename, m.linenumber,
-                    msgmap[m.id]))
-                return True
-            fieldset = set()
-            for f in m.fields:
-                if f.name in fieldset:
-                    print("ERROR: Duplicate field %s in message %s (%s:%u)" % (
-                        f.name, m.name,
-                        x.filename, m.linenumber))
-                    return True
-                fieldset.add(f.name)
-            msgmap[m.id] = '%s (%s:%u)' % (m.name, x.filename, m.linenumber)
-        for enum in x.enum:
-            for entry in enum.entry:
-                s1 = "%s.%s" % (enum.name, entry.name)
-                s2 = "%s.%s" % (enum.name, entry.value)
-                if s1 in enummap or s2 in enummap:
-                    print("ERROR: Duplicate enums %s/%s at %s:%u and %s" % (
-                        s1, entry.value, x.filename, enum.linenumber,
-                        enummap.get(s1) or enummap.get(s2)))
-                    return True
-                enummap[s1] = "%s:%u" % (x.filename, enum.linenumber)
-                enummap[s2] = "%s:%u" % (x.filename, enum.linenumber)
-                    
-    return False
-
-
-
-def total_msgs(xml):
-    '''count total number of msgs'''
-    count = 0
-    for x in xml:
-        count += len(x.message)
-    return count
-
-def mkdir_p(dir):
-    try:
-        os.makedirs(dir)
-    except OSError as exc:
-        if exc.errno == errno.EEXIST:
-            pass
-        else: raise
-
-# check version consistent
-# add test.xml
-# finish test suite
-# printf style error macro, if defined call errors
diff --git a/mavlink/share/pyshared/pymavlink/generator/mavtemplate.py b/mavlink/share/pyshared/pymavlink/generator/mavtemplate.py
deleted file mode 100644
index 6ef015315..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/mavtemplate.py
+++ /dev/null
@@ -1,121 +0,0 @@
-#!/usr/bin/env python
-'''
-simple templating system for mavlink generator
-
-Copyright Andrew Tridgell 2011
-Released under GNU GPL version 3 or later
-'''
-
-from mavparse import MAVParseError
-
-class MAVTemplate(object):
-    '''simple templating system'''
-    def __init__(self,
-                 start_var_token="${", 
-                 end_var_token="}", 
-                 start_rep_token="${{", 
-                 end_rep_token="}}",
-                 trim_leading_lf=True,
-                 checkmissing=True):
-        self.start_var_token = start_var_token
-        self.end_var_token = end_var_token
-        self.start_rep_token = start_rep_token
-        self.end_rep_token = end_rep_token
-        self.trim_leading_lf = trim_leading_lf
-        self.checkmissing = checkmissing
-
-    def find_end(self, text, start_token, end_token):
-        '''find the of a token.
-        Returns the offset in the string immediately after the matching end_token'''
-        if not text.startswith(start_token):
-            raise MAVParseError("invalid token start")
-        offset = len(start_token)
-        nesting = 1
-        while nesting > 0:
-            idx1 = text[offset:].find(start_token)
-            idx2 = text[offset:].find(end_token)
-            if idx1 == -1 and idx2 == -1:
-                raise MAVParseError("token nesting error")
-            if idx1 == -1 or idx1 > idx2:
-                offset += idx2 + len(end_token)
-                nesting -= 1
-            else:
-                offset += idx1 + len(start_token)
-                nesting += 1
-        return offset
-
-    def find_var_end(self, text):
-        '''find the of a variable'''
-        return self.find_end(text, self.start_var_token, self.end_var_token)
-
-    def find_rep_end(self, text):
-        '''find the of a repitition'''
-        return self.find_end(text, self.start_rep_token, self.end_rep_token)
-
-    def substitute(self, text, subvars={},
-                   trim_leading_lf=None, checkmissing=None):
-        '''substitute variables in a string'''
-
-        if trim_leading_lf is None:
-            trim_leading_lf = self.trim_leading_lf
-        if checkmissing is None:
-            checkmissing = self.checkmissing
-
-        # handle repititions
-        while True:
-            subidx = text.find(self.start_rep_token)
-            if subidx == -1:
-                break
-            endidx = self.find_rep_end(text[subidx:])
-            if endidx == -1:
-                raise MAVParseError("missing end macro in %s" % text[subidx:])
-            part1 = text[0:subidx]
-            part2 = text[subidx+len(self.start_rep_token):subidx+(endidx-len(self.end_rep_token))]
-            part3 = text[subidx+endidx:]
-            a = part2.split(':')
-            field_name = a[0]
-            rest = ':'.join(a[1:])
-            v = getattr(subvars, field_name, None)
-            if v is None:
-                raise MAVParseError('unable to find field %s' % field_name)
-            t1 = part1
-            for f in v:
-                t1 += self.substitute(rest, f, trim_leading_lf=False, checkmissing=False)
-            if len(v) != 0 and t1[-1] in ["\n", ","]:
-                t1 = t1[:-1]
-            t1 += part3
-            text = t1
-                
-        if trim_leading_lf:
-            if text[0] == '\n':
-                text = text[1:]
-        while True:
-            idx = text.find(self.start_var_token)
-            if idx == -1:
-                return text
-            endidx = text[idx:].find(self.end_var_token)
-            if endidx == -1:
-                raise MAVParseError('missing end of variable: %s' % text[idx:idx+10])
-            varname = text[idx+2:idx+endidx]
-            if isinstance(subvars, dict):
-                if not varname in subvars:
-                    if checkmissing:
-                        raise MAVParseError("unknown variable in '%s%s%s'" % (
-                            self.start_var_token, varname, self.end_var_token))
-                    return text[0:idx+endidx] + self.substitute(text[idx+endidx:], subvars,
-                                                                trim_leading_lf=False, checkmissing=False)
-                value = subvars[varname]
-            else:
-                value = getattr(subvars, varname, None)
-                if value is None:
-                    if checkmissing:
-                        raise MAVParseError("unknown variable in '%s%s%s'" % (
-                            self.start_var_token, varname, self.end_var_token))
-                    return text[0:idx+endidx] + self.substitute(text[idx+endidx:], subvars,
-                                                                trim_leading_lf=False, checkmissing=False)
-            text = text.replace("%s%s%s" % (self.start_var_token, varname, self.end_var_token), str(value))
-        return text
-
-    def write(self, file, text, subvars={}, trim_leading_lf=True):
-        '''write to a file with variable substitution'''
-        file.write(self.substitute(text, subvars=subvars, trim_leading_lf=trim_leading_lf))
diff --git a/mavlink/share/pyshared/pymavlink/generator/mavtestgen.py b/mavlink/share/pyshared/pymavlink/generator/mavtestgen.py
deleted file mode 100644
index faffa1c19..000000000
--- a/mavlink/share/pyshared/pymavlink/generator/mavtestgen.py
+++ /dev/null
@@ -1,142 +0,0 @@
-#!/usr/bin/env python
-'''
-generate a MAVLink test suite
-
-Copyright Andrew Tridgell 2011
-Released under GNU GPL version 3 or later
-'''
-
-import sys, textwrap
-from optparse import OptionParser
-
-# mavparse is up a directory level
-sys.path.append('..')
-import mavparse
-
-def gen_value(f, i, language):
-    '''generate a test value for the ith field of a message'''
-    type = f.type
-
-    # could be an array
-    if type.find("[") != -1:
-        aidx = type.find("[")
-        basetype = type[0:aidx]
-        if basetype == "array":
-            basetype = "int8_t"
-        if language == 'C':
-            return '(const %s *)"%s%u"' % (basetype, f.name, i)
-        return '"%s%u"' % (f.name, i)
-
-    if type == 'float':
-        return 17.0 + i*7
-    if type == 'char':
-        return 'A' + i
-    if type == 'int8_t':
-        return 5 + i
-    if type in ['int8_t', 'uint8_t']:
-        return 5 + i
-    if type in ['uint8_t_mavlink_version']:
-        return 2
-    if type in ['int16_t', 'uint16_t']:
-        return 17235 + i*52
-    if type in ['int32_t', 'uint32_t']:
-        v = 963497464 + i*52
-        if language == 'C':
-            return "%sL" % v
-        return v
-    if type in ['int64_t', 'uint64_t']:
-        v = 9223372036854775807 + i*63
-        if language == 'C':
-            return "%sLL" % v
-        return v
-
-
-
-def generate_methods_python(outf, msgs):
-    outf.write("""
-'''
-MAVLink protocol test implementation (auto-generated by mavtestgen.py)
-
-Generated from: %s
-
-Note: this file has been auto-generated. DO NOT EDIT
-'''
-
-import mavlink
-
-def generate_outputs(mav):
-    '''generate all message types as outputs'''
-""")
-    for m in msgs:
-        if m.name == "HEARTBEAT": continue
-        outf.write("\tmav.%s_send(" % m.name.lower())
-        for i in range(0, len(m.fields)):
-            f = m.fields[i]
-            outf.write("%s=%s" % (f.name, gen_value(f, i, 'py')))
-            if i != len(m.fields)-1:
-                outf.write(",")
-        outf.write(")\n")
-
-
-def generate_methods_C(outf, msgs):
-    outf.write("""
-/*
-MAVLink protocol test implementation (auto-generated by mavtestgen.py)
-
-Generated from: %s
-
-Note: this file has been auto-generated. DO NOT EDIT
-*/
-
-static void mavtest_generate_outputs(mavlink_channel_t chan)
-{
-""")
-    for m in msgs:
-        if m.name == "HEARTBEAT": continue
-        outf.write("\tmavlink_msg_%s_send(chan," % m.name.lower())
-        for i in range(0, len(m.fields)):
-            f = m.fields[i]
-            outf.write("%s" % gen_value(f, i, 'C'))
-            if i != len(m.fields)-1:
-                outf.write(",")
-        outf.write(");\n")
-    outf.write("}\n")
-
-
-
-######################################################################
-'''main program'''
-
-parser = OptionParser("%prog [options] <XML files>")
-parser.add_option("-o", "--output", dest="output", default="mavtest", help="output folder [default: %default]")
-(opts, args) = parser.parse_args()
-
-if len(args) < 1:
-    parser.error("You must supply at least one MAVLink XML protocol definition")
-    
-
-msgs = []
-enums = []
-
-for fname in args:
-    (m, e) = mavparse.parse_mavlink_xml(fname)
-    msgs.extend(m)
-    enums.extend(e)
-
-
-if mavparse.check_duplicates(msgs):
-    sys.exit(1)
-
-print("Found %u MAVLink message types" % len(msgs))
-
-print("Generating python %s" % (opts.output+'.py'))
-outf = open(opts.output + '.py', "w")
-generate_methods_python(outf, msgs)
-outf.close()
-
-print("Generating C %s" % (opts.output+'.h'))
-outf = open(opts.output + '.h', "w")
-generate_methods_C(outf, msgs)
-outf.close()
-
-print("Generated %s OK" % opts.output)
diff --git a/mavlink/share/pyshared/pymavlink/mavextra.py b/mavlink/share/pyshared/pymavlink/mavextra.py
deleted file mode 100644
index 5395f6036..000000000
--- a/mavlink/share/pyshared/pymavlink/mavextra.py
+++ /dev/null
@@ -1,154 +0,0 @@
-#!/usr/bin/env python
-'''
-useful extra functions for use by mavlink clients
-
-Copyright Andrew Tridgell 2011
-Released under GNU GPL version 3 or later
-'''
-
-from math import *
-
-
-def kmh(mps):
-    '''convert m/s to Km/h'''
-    return mps*3.6
-
-def altitude(press_abs, ground_press=955.0, ground_temp=30):
-    '''calculate barometric altitude'''
-    return log(ground_press/press_abs)*(ground_temp+273.15)*29271.267*0.001
-
-
-def mag_heading(RAW_IMU, ATTITUDE, declination=0, SENSOR_OFFSETS=None, ofs=None):
-    '''calculate heading from raw magnetometer'''
-    mag_x = RAW_IMU.xmag
-    mag_y = RAW_IMU.ymag
-    mag_z = RAW_IMU.zmag
-    if SENSOR_OFFSETS is not None and ofs is not None:
-        mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
-        mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
-        mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
-
-    headX = mag_x*cos(ATTITUDE.pitch) + mag_y*sin(ATTITUDE.roll)*sin(ATTITUDE.pitch) + mag_z*cos(ATTITUDE.roll)*sin(ATTITUDE.pitch)
-    headY = mag_y*cos(ATTITUDE.roll) - mag_z*sin(ATTITUDE.roll)
-    heading = degrees(atan2(-headY,headX)) + declination
-    if heading < 0:
-        heading += 360
-    return heading
-
-def mag_field(RAW_IMU, SENSOR_OFFSETS=None, ofs=None):
-    '''calculate magnetic field strength from raw magnetometer'''
-    mag_x = RAW_IMU.xmag
-    mag_y = RAW_IMU.ymag
-    mag_z = RAW_IMU.zmag
-    if SENSOR_OFFSETS is not None and ofs is not None:
-        mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
-        mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
-        mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
-    return sqrt(mag_x**2 + mag_y**2 + mag_z**2)
-
-def angle_diff(angle1, angle2):
-    '''show the difference between two angles in degrees'''
-    ret = angle1 - angle2
-    if ret > 180:
-        ret -= 360;
-    if ret < -180:
-        ret += 360
-    return ret
-
-
-lowpass_data = {}
-
-def lowpass(var, key, factor):
-    '''a simple lowpass filter'''
-    global lowpass_data
-    if not key in lowpass_data:
-        lowpass_data[key] = var
-    else:
-        lowpass_data[key] = factor*lowpass_data[key] + (1.0 - factor)*var
-    return lowpass_data[key]
-
-last_delta = {}
-
-def delta(var, key):
-    '''calculate slope'''
-    global last_delta
-    dv = 0
-    if key in last_delta:
-        dv = var - last_delta[key]
-    last_delta[key] = var
-    return dv
-
-def delta_angle(var, key):
-    '''calculate slope of an angle'''
-    global last_delta
-    dv = 0
-    if key in last_delta:
-        dv = var - last_delta[key]
-    last_delta[key] = var
-    if dv > 180:
-        dv -= 360
-    if dv < -180:
-        dv += 360
-    return dv
-
-def roll_estimate(RAW_IMU,smooth=0.7):
-    '''estimate roll from accelerometer'''
-    rx = lowpass(RAW_IMU.xacc,'rx',smooth)
-    ry = lowpass(RAW_IMU.yacc,'ry',smooth)
-    rz = lowpass(RAW_IMU.zacc,'rz',smooth)
-    return degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2)))
-
-def pitch_estimate(RAW_IMU, smooth=0.7):
-    '''estimate pitch from accelerometer'''
-    rx = lowpass(RAW_IMU.xacc,'rx',smooth)
-    ry = lowpass(RAW_IMU.yacc,'ry',smooth)
-    rz = lowpass(RAW_IMU.zacc,'rz',smooth)
-    return degrees(asin(rx/sqrt(rx**2+ry**2+rz**2)))
-
-def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, smooth=0.7):
-    '''estimate pitch from accelerometer'''
-    rx = RAW_IMU.xacc
-    ry = RAW_IMU.yacc
-    rz = RAW_IMU.zacc+45
-    if SENSOR_OFFSETS is not None and ofs is not None:
-        rx += ofs[0] - SENSOR_OFFSETS.accel_cal_x
-        ry += ofs[1] - SENSOR_OFFSETS.accel_cal_y
-        rz += ofs[2] - SENSOR_OFFSETS.accel_cal_z
-    return lowpass(sqrt(rx**2+ry**2+rz**2)*0.01,'_gravity',smooth)
-
-
-
-def pitch_sim(SIMSTATE, GPS_RAW):
-    '''estimate pitch from SIMSTATE accels'''
-    xacc = SIMSTATE.xacc - lowpass(delta(GPS_RAW.v,"v")*6.6, "v", 0.9)
-    zacc = SIMSTATE.zacc
-    zacc += SIMSTATE.ygyro * GPS_RAW.v;
-    if xacc/zacc >= 1:
-        return 0
-    if xacc/zacc <= -1:
-        return -0
-    return degrees(-asin(xacc/zacc))
-
-def distance_two(GPS_RAW1, GPS_RAW2):
-    '''distance between two points'''
-    lat1 = radians(GPS_RAW1.lat)
-    lat2 = radians(GPS_RAW2.lat)
-    lon1 = radians(GPS_RAW1.lon)
-    lon2 = radians(GPS_RAW2.lon)
-    dLat = lat2 - lat1
-    dLon = lon2 - lon1
-
-    a = sin(0.5*dLat) * sin(0.5*dLat) + sin(0.5*dLon) * sin(0.5*dLon) * cos(lat1) * cos(lat2)
-    c = 2.0 * atan2(sqrt(a), sqrt(1.0-a))
-    return 6371 * 1000 * c
-
-
-first_fix = None
-
-def distance_home(GPS_RAW):
-    '''distance from first fix point'''
-    global first_fix
-    if first_fix == None or first_fix.fix_type < 2:
-        first_fix = GPS_RAW
-        return 0
-    return distance_two(GPS_RAW, first_fix)
diff --git a/mavlink/share/pyshared/pymavlink/mavlink.py b/mavlink/share/pyshared/pymavlink/mavlink.py
deleted file mode 100644
index 3287a921d..000000000
--- a/mavlink/share/pyshared/pymavlink/mavlink.py
+++ /dev/null
@@ -1,4930 +0,0 @@
-'''
-MAVLink protocol implementation (auto-generated by mavgen.py)
-
-Generated from: ardupilotmega.xml,common.xml
-
-Note: this file has been auto-generated. DO NOT EDIT
-'''
-
-import struct, array, mavutil, time
-
-WIRE_PROTOCOL_VERSION = "0.9"
-
-class MAVLink_header(object):
-    '''MAVLink message header'''
-    def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
-        self.mlen = mlen
-        self.seq = seq
-        self.srcSystem = srcSystem
-        self.srcComponent = srcComponent
-        self.msgId = msgId
-
-    def pack(self):
-        return struct.pack('BBBBBB', 85, self.mlen, self.seq,
-                          self.srcSystem, self.srcComponent, self.msgId)
-
-class MAVLink_message(object):
-    '''base MAVLink message class'''
-    def __init__(self, msgId, name):
-        self._header     = MAVLink_header(msgId)
-        self._payload    = None
-        self._msgbuf     = None
-        self._crc        = None
-        self._fieldnames = []
-        self._type       = name
-
-    def get_msgbuf(self):
-        return self._msgbuf
-
-    def get_header(self):
-        return self._header
-
-    def get_payload(self):
-        return self._payload
-
-    def get_crc(self):
-        return self._crc
-
-    def get_fieldnames(self):
-        return self._fieldnames
-
-    def get_type(self):
-        return self._type
-
-    def get_msgId(self):
-        return self._header.msgId
-
-    def get_srcSystem(self):
-        return self._header.srcSystem
-
-    def get_srcComponent(self):
-        return self._header.srcComponent
-
-    def get_seq(self):
-        return self._header.seq
-
-    def __str__(self):
-        ret = '%s {' % self._type
-        for a in self._fieldnames:
-            v = getattr(self, a)
-            ret += '%s : %s, ' % (a, v)
-        ret = ret[0:-2] + '}'
-        return ret            
-
-    def pack(self, mav, crc_extra, payload):
-        self._payload = payload
-        self._header  = MAVLink_header(self._header.msgId, len(payload), mav.seq,
-                                       mav.srcSystem, mav.srcComponent)
-        self._msgbuf = self._header.pack() + payload
-        crc = mavutil.x25crc(self._msgbuf[1:])
-        if False: # using CRC extra
-            crc.accumulate(chr(crc_extra))
-        self._crc = crc.crc
-        self._msgbuf += struct.pack('<H', self._crc)
-        return self._msgbuf
-
-
-# enums
-
-# 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 # 
-
-# MAV_CMD
-MAV_CMD_NAV_WAYPOINT = 16 # Navigate to waypoint.
-MAV_CMD_NAV_LOITER_UNLIM = 17 # Loiter around this waypoint an unlimited amount of time
-MAV_CMD_NAV_LOITER_TURNS = 18 # Loiter around this waypoint for X turns
-MAV_CMD_NAV_LOITER_TIME = 19 # Loiter around this waypoint for X seconds
-MAV_CMD_NAV_RETURN_TO_LAUNCH = 20 # Return to launch location
-MAV_CMD_NAV_LAND = 21 # Land at location
-MAV_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand
-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.
-MAV_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV.
-MAV_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the
-                        # NAV/ACTION commands in the enumeration
-MAV_CMD_CONDITION_DELAY = 112 # Delay mission state machine.
-MAV_CMD_CONDITION_CHANGE_ALT = 113 # Ascend/descend at rate.  Delay mission state machine until desired
-                        # altitude reached.
-MAV_CMD_CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV
-                        # point.
-MAV_CMD_CONDITION_YAW = 115 # Reach a certain target angle.
-MAV_CMD_CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the
-                        # CONDITION commands in the enumeration
-MAV_CMD_DO_SET_MODE = 176 # Set system mode.
-MAV_CMD_DO_JUMP = 177 # Jump to the desired command in the mission list.  Repeat this action
-                        # only the specified number of times
-MAV_CMD_DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points.
-MAV_CMD_DO_SET_HOME = 179 # Changes the home location either to the current location or a
-                        # specified location.
-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.
-MAV_CMD_DO_SET_RELAY = 181 # Set a relay to a condition.
-MAV_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cyles with a desired
-                        # period.
-MAV_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value.
-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.
-MAV_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera capturing.
-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
-                        # devices such as cameras.
-MAV_CMD_DO_DIGICAM_CONFIGURE = 202 # Mission command to configure an on-board camera controller system.
-MAV_CMD_DO_DIGICAM_CONTROL = 203 # Mission command to control an on-board camera controller system.
-MAV_CMD_DO_MOUNT_CONFIGURE = 204 # Mission command to configure a camera or antenna mount
-MAV_CMD_DO_MOUNT_CONTROL = 205 # Mission command to control a camera or antenna mount
-MAV_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO
-                        # commands in the enumeration
-MAV_CMD_PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-
-                        # flight mode.
-MAV_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command
-                        # will be only accepted if in pre-flight mode.
-MAV_CMD_ENUM_END = 246 # 
-
-# 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_ENUM_END = 2 # 
-
-# FENCE_BREACH
-FENCE_BREACH_NONE = 0 # No last fence breach
-FENCE_BREACH_MINALT = 1 # Breached minimum altitude
-FENCE_BREACH_MAXALT = 2 # Breached minimum altitude
-FENCE_BREACH_BOUNDARY = 3 # Breached fence boundary
-FENCE_BREACH_ENUM_END = 4 # 
-
-# MAV_DATA_STREAM
-MAV_DATA_STREAM_ALL = 0 # Enable all data streams
-MAV_DATA_STREAM_RAW_SENSORS = 1 # Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
-MAV_DATA_STREAM_EXTENDED_STATUS = 2 # Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS
-MAV_DATA_STREAM_RC_CHANNELS = 3 # Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW
-MAV_DATA_STREAM_RAW_CONTROLLER = 4 # Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT,
-                        # NAV_CONTROLLER_OUTPUT.
-MAV_DATA_STREAM_POSITION = 6 # Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.
-MAV_DATA_STREAM_EXTRA1 = 10 # Dependent on the autopilot
-MAV_DATA_STREAM_EXTRA2 = 11 # Dependent on the autopilot
-MAV_DATA_STREAM_EXTRA3 = 12 # Dependent on the autopilot
-MAV_DATA_STREAM_ENUM_END = 13 # 
-
-# MAV_ROI
-MAV_ROI_NONE = 0 # No region of interest.
-MAV_ROI_WPNEXT = 1 # Point toward next waypoint.
-MAV_ROI_WPINDEX = 2 # Point toward given waypoint.
-MAV_ROI_LOCATION = 3 # Point toward fixed location.
-MAV_ROI_TARGET = 4 # Point toward of given id.
-MAV_ROI_ENUM_END = 5 # 
-
-# message IDs
-MAVLINK_MSG_ID_BAD_DATA = -1
-MAVLINK_MSG_ID_SENSOR_OFFSETS = 150
-MAVLINK_MSG_ID_SET_MAG_OFFSETS = 151
-MAVLINK_MSG_ID_MEMINFO = 152
-MAVLINK_MSG_ID_AP_ADC = 153
-MAVLINK_MSG_ID_DIGICAM_CONFIGURE = 154
-MAVLINK_MSG_ID_DIGICAM_CONTROL = 155
-MAVLINK_MSG_ID_MOUNT_CONFIGURE = 156
-MAVLINK_MSG_ID_MOUNT_CONTROL = 157
-MAVLINK_MSG_ID_MOUNT_STATUS = 158
-MAVLINK_MSG_ID_FENCE_POINT = 160
-MAVLINK_MSG_ID_FENCE_FETCH_POINT = 161
-MAVLINK_MSG_ID_FENCE_STATUS = 162
-MAVLINK_MSG_ID_AHRS = 163
-MAVLINK_MSG_ID_SIMSTATE = 164
-MAVLINK_MSG_ID_HWSTATUS = 165
-MAVLINK_MSG_ID_RADIO = 166
-MAVLINK_MSG_ID_HEARTBEAT = 0
-MAVLINK_MSG_ID_BOOT = 1
-MAVLINK_MSG_ID_SYSTEM_TIME = 2
-MAVLINK_MSG_ID_PING = 3
-MAVLINK_MSG_ID_SYSTEM_TIME_UTC = 4
-MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL = 5
-MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK = 6
-MAVLINK_MSG_ID_AUTH_KEY = 7
-MAVLINK_MSG_ID_ACTION_ACK = 9
-MAVLINK_MSG_ID_ACTION = 10
-MAVLINK_MSG_ID_SET_MODE = 11
-MAVLINK_MSG_ID_SET_NAV_MODE = 12
-MAVLINK_MSG_ID_PARAM_REQUEST_READ = 20
-MAVLINK_MSG_ID_PARAM_REQUEST_LIST = 21
-MAVLINK_MSG_ID_PARAM_VALUE = 22
-MAVLINK_MSG_ID_PARAM_SET = 23
-MAVLINK_MSG_ID_GPS_RAW_INT = 25
-MAVLINK_MSG_ID_SCALED_IMU = 26
-MAVLINK_MSG_ID_GPS_STATUS = 27
-MAVLINK_MSG_ID_RAW_IMU = 28
-MAVLINK_MSG_ID_RAW_PRESSURE = 29
-MAVLINK_MSG_ID_SCALED_PRESSURE = 38
-MAVLINK_MSG_ID_ATTITUDE = 30
-MAVLINK_MSG_ID_LOCAL_POSITION = 31
-MAVLINK_MSG_ID_GLOBAL_POSITION = 33
-MAVLINK_MSG_ID_GPS_RAW = 32
-MAVLINK_MSG_ID_SYS_STATUS = 34
-MAVLINK_MSG_ID_RC_CHANNELS_RAW = 35
-MAVLINK_MSG_ID_RC_CHANNELS_SCALED = 36
-MAVLINK_MSG_ID_SERVO_OUTPUT_RAW = 37
-MAVLINK_MSG_ID_WAYPOINT = 39
-MAVLINK_MSG_ID_WAYPOINT_REQUEST = 40
-MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT = 41
-MAVLINK_MSG_ID_WAYPOINT_CURRENT = 42
-MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST = 43
-MAVLINK_MSG_ID_WAYPOINT_COUNT = 44
-MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL = 45
-MAVLINK_MSG_ID_WAYPOINT_REACHED = 46
-MAVLINK_MSG_ID_WAYPOINT_ACK = 47
-MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN = 48
-MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET = 49
-MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET = 50
-MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT = 51
-MAVLINK_MSG_ID_CONTROL_STATUS = 52
-MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA = 53
-MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA = 54
-MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST = 55
-MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST = 56
-MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT = 57
-MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT = 58
-MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT = 62
-MAVLINK_MSG_ID_POSITION_TARGET = 63
-MAVLINK_MSG_ID_STATE_CORRECTION = 64
-MAVLINK_MSG_ID_SET_ALTITUDE = 65
-MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66
-MAVLINK_MSG_ID_HIL_STATE = 67
-MAVLINK_MSG_ID_HIL_CONTROLS = 68
-MAVLINK_MSG_ID_MANUAL_CONTROL = 69
-MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE = 70
-MAVLINK_MSG_ID_GLOBAL_POSITION_INT = 73
-MAVLINK_MSG_ID_VFR_HUD = 74
-MAVLINK_MSG_ID_COMMAND = 75
-MAVLINK_MSG_ID_COMMAND_ACK = 76
-MAVLINK_MSG_ID_OPTICAL_FLOW = 100
-MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT = 140
-MAVLINK_MSG_ID_DEBUG_VECT = 251
-MAVLINK_MSG_ID_NAMED_VALUE_FLOAT = 252
-MAVLINK_MSG_ID_NAMED_VALUE_INT = 253
-MAVLINK_MSG_ID_STATUSTEXT = 254
-MAVLINK_MSG_ID_DEBUG = 255
-
-class MAVLink_sensor_offsets_message(MAVLink_message):
-        '''
-        Offsets and calibrations values for hardware         sensors.
-        This makes it easier to debug the calibration process.
-        '''
-        def __init__(self, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SENSOR_OFFSETS, 'SENSOR_OFFSETS')
-                self._fieldnames = ['mag_ofs_x', 'mag_ofs_y', 'mag_ofs_z', 'mag_declination', 'raw_press', 'raw_temp', 'gyro_cal_x', 'gyro_cal_y', 'gyro_cal_z', 'accel_cal_x', 'accel_cal_y', 'accel_cal_z']
-                self.mag_ofs_x = mag_ofs_x
-                self.mag_ofs_y = mag_ofs_y
-                self.mag_ofs_z = mag_ofs_z
-                self.mag_declination = mag_declination
-                self.raw_press = raw_press
-                self.raw_temp = raw_temp
-                self.gyro_cal_x = gyro_cal_x
-                self.gyro_cal_y = gyro_cal_y
-                self.gyro_cal_z = gyro_cal_z
-                self.accel_cal_x = accel_cal_x
-                self.accel_cal_y = accel_cal_y
-                self.accel_cal_z = accel_cal_z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 143, struct.pack('>hhhfiiffffff', self.mag_ofs_x, self.mag_ofs_y, self.mag_ofs_z, self.mag_declination, self.raw_press, self.raw_temp, self.gyro_cal_x, self.gyro_cal_y, self.gyro_cal_z, self.accel_cal_x, self.accel_cal_y, self.accel_cal_z))
-
-class MAVLink_set_mag_offsets_message(MAVLink_message):
-        '''
-        set the magnetometer offsets
-        '''
-        def __init__(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_MAG_OFFSETS, 'SET_MAG_OFFSETS')
-                self._fieldnames = ['target_system', 'target_component', 'mag_ofs_x', 'mag_ofs_y', 'mag_ofs_z']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.mag_ofs_x = mag_ofs_x
-                self.mag_ofs_y = mag_ofs_y
-                self.mag_ofs_z = mag_ofs_z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 29, struct.pack('>BBhhh', self.target_system, self.target_component, self.mag_ofs_x, self.mag_ofs_y, self.mag_ofs_z))
-
-class MAVLink_meminfo_message(MAVLink_message):
-        '''
-        state of APM memory
-        '''
-        def __init__(self, brkval, freemem):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MEMINFO, 'MEMINFO')
-                self._fieldnames = ['brkval', 'freemem']
-                self.brkval = brkval
-                self.freemem = freemem
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 208, struct.pack('>HH', self.brkval, self.freemem))
-
-class MAVLink_ap_adc_message(MAVLink_message):
-        '''
-        raw ADC output
-        '''
-        def __init__(self, adc1, adc2, adc3, adc4, adc5, adc6):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_AP_ADC, 'AP_ADC')
-                self._fieldnames = ['adc1', 'adc2', 'adc3', 'adc4', 'adc5', 'adc6']
-                self.adc1 = adc1
-                self.adc2 = adc2
-                self.adc3 = adc3
-                self.adc4 = adc4
-                self.adc5 = adc5
-                self.adc6 = adc6
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 188, struct.pack('>HHHHHH', self.adc1, self.adc2, self.adc3, self.adc4, self.adc5, self.adc6))
-
-class MAVLink_digicam_configure_message(MAVLink_message):
-        '''
-        Configure on-board Camera Control System.
-        '''
-        def __init__(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, 'DIGICAM_CONFIGURE')
-                self._fieldnames = ['target_system', 'target_component', 'mode', 'shutter_speed', 'aperture', 'iso', 'exposure_type', 'command_id', 'engine_cut_off', 'extra_param', 'extra_value']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.mode = mode
-                self.shutter_speed = shutter_speed
-                self.aperture = aperture
-                self.iso = iso
-                self.exposure_type = exposure_type
-                self.command_id = command_id
-                self.engine_cut_off = engine_cut_off
-                self.extra_param = extra_param
-                self.extra_value = extra_value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 118, struct.pack('>BBBHBBBBBBf', self.target_system, self.target_component, self.mode, self.shutter_speed, self.aperture, self.iso, self.exposure_type, self.command_id, self.engine_cut_off, self.extra_param, self.extra_value))
-
-class MAVLink_digicam_control_message(MAVLink_message):
-        '''
-        Control on-board Camera Control System to take shots.
-        '''
-        def __init__(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DIGICAM_CONTROL, 'DIGICAM_CONTROL')
-                self._fieldnames = ['target_system', 'target_component', 'session', 'zoom_pos', 'zoom_step', 'focus_lock', 'shot', 'command_id', 'extra_param', 'extra_value']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.session = session
-                self.zoom_pos = zoom_pos
-                self.zoom_step = zoom_step
-                self.focus_lock = focus_lock
-                self.shot = shot
-                self.command_id = command_id
-                self.extra_param = extra_param
-                self.extra_value = extra_value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 242, struct.pack('>BBBBbBBBBf', self.target_system, self.target_component, self.session, self.zoom_pos, self.zoom_step, self.focus_lock, self.shot, self.command_id, self.extra_param, self.extra_value))
-
-class MAVLink_mount_configure_message(MAVLink_message):
-        '''
-        Message to configure a camera mount, directional antenna, etc.
-        '''
-        def __init__(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_CONFIGURE, 'MOUNT_CONFIGURE')
-                self._fieldnames = ['target_system', 'target_component', 'mount_mode', 'stab_roll', 'stab_pitch', 'stab_yaw']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.mount_mode = mount_mode
-                self.stab_roll = stab_roll
-                self.stab_pitch = stab_pitch
-                self.stab_yaw = stab_yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 19, struct.pack('>BBBBBB', self.target_system, self.target_component, self.mount_mode, self.stab_roll, self.stab_pitch, self.stab_yaw))
-
-class MAVLink_mount_control_message(MAVLink_message):
-        '''
-        Message to control a camera mount, directional antenna, etc.
-        '''
-        def __init__(self, target_system, target_component, input_a, input_b, input_c, save_position):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_CONTROL, 'MOUNT_CONTROL')
-                self._fieldnames = ['target_system', 'target_component', 'input_a', 'input_b', 'input_c', 'save_position']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.input_a = input_a
-                self.input_b = input_b
-                self.input_c = input_c
-                self.save_position = save_position
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 97, struct.pack('>BBiiiB', self.target_system, self.target_component, self.input_a, self.input_b, self.input_c, self.save_position))
-
-class MAVLink_mount_status_message(MAVLink_message):
-        '''
-        Message with some status from APM to GCS about camera or
-        antenna mount
-        '''
-        def __init__(self, target_system, target_component, pointing_a, pointing_b, pointing_c):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_STATUS, 'MOUNT_STATUS')
-                self._fieldnames = ['target_system', 'target_component', 'pointing_a', 'pointing_b', 'pointing_c']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.pointing_a = pointing_a
-                self.pointing_b = pointing_b
-                self.pointing_c = pointing_c
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 233, struct.pack('>BBiii', self.target_system, self.target_component, self.pointing_a, self.pointing_b, self.pointing_c))
-
-class MAVLink_fence_point_message(MAVLink_message):
-        '''
-        A fence point. Used to set a point when from               GCS
-        -> MAV. Also used to return a point from MAV -> GCS
-        '''
-        def __init__(self, target_system, target_component, idx, count, lat, lng):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_POINT, 'FENCE_POINT')
-                self._fieldnames = ['target_system', 'target_component', 'idx', 'count', 'lat', 'lng']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.idx = idx
-                self.count = count
-                self.lat = lat
-                self.lng = lng
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 18, struct.pack('>BBBBff', self.target_system, self.target_component, self.idx, self.count, self.lat, self.lng))
-
-class MAVLink_fence_fetch_point_message(MAVLink_message):
-        '''
-        Request a current fence point from MAV
-        '''
-        def __init__(self, target_system, target_component, idx):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_FETCH_POINT, 'FENCE_FETCH_POINT')
-                self._fieldnames = ['target_system', 'target_component', 'idx']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.idx = idx
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 68, struct.pack('>BBB', self.target_system, self.target_component, self.idx))
-
-class MAVLink_fence_status_message(MAVLink_message):
-        '''
-        Status of geo-fencing. Sent in extended             status
-        stream when fencing enabled
-        '''
-        def __init__(self, breach_status, breach_count, breach_type, breach_time):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_STATUS, 'FENCE_STATUS')
-                self._fieldnames = ['breach_status', 'breach_count', 'breach_type', 'breach_time']
-                self.breach_status = breach_status
-                self.breach_count = breach_count
-                self.breach_type = breach_type
-                self.breach_time = breach_time
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 136, struct.pack('>BHBI', self.breach_status, self.breach_count, self.breach_type, self.breach_time))
-
-class MAVLink_ahrs_message(MAVLink_message):
-        '''
-        Status of DCM attitude estimator
-        '''
-        def __init__(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_AHRS, 'AHRS')
-                self._fieldnames = ['omegaIx', 'omegaIy', 'omegaIz', 'accel_weight', 'renorm_val', 'error_rp', 'error_yaw']
-                self.omegaIx = omegaIx
-                self.omegaIy = omegaIy
-                self.omegaIz = omegaIz
-                self.accel_weight = accel_weight
-                self.renorm_val = renorm_val
-                self.error_rp = error_rp
-                self.error_yaw = error_yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 127, struct.pack('>fffffff', self.omegaIx, self.omegaIy, self.omegaIz, self.accel_weight, self.renorm_val, self.error_rp, self.error_yaw))
-
-class MAVLink_simstate_message(MAVLink_message):
-        '''
-        Status of simulation environment, if used
-        '''
-        def __init__(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SIMSTATE, 'SIMSTATE')
-                self._fieldnames = ['roll', 'pitch', 'yaw', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro']
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.xacc = xacc
-                self.yacc = yacc
-                self.zacc = zacc
-                self.xgyro = xgyro
-                self.ygyro = ygyro
-                self.zgyro = zgyro
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 42, struct.pack('>fffffffff', self.roll, self.pitch, self.yaw, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro))
-
-class MAVLink_hwstatus_message(MAVLink_message):
-        '''
-        Status of key hardware
-        '''
-        def __init__(self, Vcc, I2Cerr):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HWSTATUS, 'HWSTATUS')
-                self._fieldnames = ['Vcc', 'I2Cerr']
-                self.Vcc = Vcc
-                self.I2Cerr = I2Cerr
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 21, struct.pack('>HB', self.Vcc, self.I2Cerr))
-
-class MAVLink_radio_message(MAVLink_message):
-        '''
-        Status generated by radio
-        '''
-        def __init__(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RADIO, 'RADIO')
-                self._fieldnames = ['rssi', 'remrssi', 'txbuf', 'noise', 'remnoise', 'rxerrors', 'fixed']
-                self.rssi = rssi
-                self.remrssi = remrssi
-                self.txbuf = txbuf
-                self.noise = noise
-                self.remnoise = remnoise
-                self.rxerrors = rxerrors
-                self.fixed = fixed
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 93, struct.pack('>BBBBBHH', self.rssi, self.remrssi, self.txbuf, self.noise, self.remnoise, self.rxerrors, self.fixed))
-
-class MAVLink_heartbeat_message(MAVLink_message):
-        '''
-        The heartbeat message shows that a system is present and
-        responding. The type of the MAV and Autopilot hardware allow
-        the receiving system to treat further messages from this
-        system appropriate (e.g. by laying out the user interface
-        based on the autopilot).
-        '''
-        def __init__(self, type, autopilot, mavlink_version):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HEARTBEAT, 'HEARTBEAT')
-                self._fieldnames = ['type', 'autopilot', 'mavlink_version']
-                self.type = type
-                self.autopilot = autopilot
-                self.mavlink_version = mavlink_version
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 72, struct.pack('>BBB', self.type, self.autopilot, self.mavlink_version))
-
-class MAVLink_boot_message(MAVLink_message):
-        '''
-        The boot message indicates that a system is starting. The
-        onboard software version allows to keep track of onboard
-        soft/firmware revisions.
-        '''
-        def __init__(self, version):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_BOOT, 'BOOT')
-                self._fieldnames = ['version']
-                self.version = version
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 39, struct.pack('>I', self.version))
-
-class MAVLink_system_time_message(MAVLink_message):
-        '''
-        The system time is the time of the master clock, typically the
-        computer clock of the main onboard computer.
-        '''
-        def __init__(self, time_usec):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYSTEM_TIME, 'SYSTEM_TIME')
-                self._fieldnames = ['time_usec']
-                self.time_usec = time_usec
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 190, struct.pack('>Q', self.time_usec))
-
-class MAVLink_ping_message(MAVLink_message):
-        '''
-        A ping message either requesting or responding to a ping. This
-        allows to measure the system latencies, including serial port,
-        radio modem and UDP connections.
-        '''
-        def __init__(self, seq, target_system, target_component, time):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PING, 'PING')
-                self._fieldnames = ['seq', 'target_system', 'target_component', 'time']
-                self.seq = seq
-                self.target_system = target_system
-                self.target_component = target_component
-                self.time = time
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 92, struct.pack('>IBBQ', self.seq, self.target_system, self.target_component, self.time))
-
-class MAVLink_system_time_utc_message(MAVLink_message):
-        '''
-        UTC date and time from GPS module
-        '''
-        def __init__(self, utc_date, utc_time):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, 'SYSTEM_TIME_UTC')
-                self._fieldnames = ['utc_date', 'utc_time']
-                self.utc_date = utc_date
-                self.utc_time = utc_time
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 191, struct.pack('>II', self.utc_date, self.utc_time))
-
-class MAVLink_change_operator_control_message(MAVLink_message):
-        '''
-        Request to control this MAV
-        '''
-        def __init__(self, target_system, control_request, version, passkey):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, 'CHANGE_OPERATOR_CONTROL')
-                self._fieldnames = ['target_system', 'control_request', 'version', 'passkey']
-                self.target_system = target_system
-                self.control_request = control_request
-                self.version = version
-                self.passkey = passkey
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 217, struct.pack('>BBB25s', self.target_system, self.control_request, self.version, self.passkey))
-
-class MAVLink_change_operator_control_ack_message(MAVLink_message):
-        '''
-        Accept / deny control of this MAV
-        '''
-        def __init__(self, gcs_system_id, control_request, ack):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, 'CHANGE_OPERATOR_CONTROL_ACK')
-                self._fieldnames = ['gcs_system_id', 'control_request', 'ack']
-                self.gcs_system_id = gcs_system_id
-                self.control_request = control_request
-                self.ack = ack
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 104, struct.pack('>BBB', self.gcs_system_id, self.control_request, self.ack))
-
-class MAVLink_auth_key_message(MAVLink_message):
-        '''
-        Emit an encrypted signature / key identifying this system.
-        PLEASE NOTE: This protocol has been kept simple, so
-        transmitting the key requires an encrypted channel for true
-        safety.
-        '''
-        def __init__(self, key):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_AUTH_KEY, 'AUTH_KEY')
-                self._fieldnames = ['key']
-                self.key = key
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 119, struct.pack('>32s', self.key))
-
-class MAVLink_action_ack_message(MAVLink_message):
-        '''
-        This message acknowledges an action. IMPORTANT: The
-        acknowledgement can be also negative, e.g. the MAV rejects a
-        reset message because it is in-flight. The action ids are
-        defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h
-        '''
-        def __init__(self, action, result):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ACTION_ACK, 'ACTION_ACK')
-                self._fieldnames = ['action', 'result']
-                self.action = action
-                self.result = result
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 219, struct.pack('>BB', self.action, self.result))
-
-class MAVLink_action_message(MAVLink_message):
-        '''
-        An action message allows to execute a certain onboard action.
-        These include liftoff, land, storing parameters too EEPROM,
-        shutddown, etc. The action ids are defined in ENUM MAV_ACTION
-        in mavlink/include/mavlink_types.h
-        '''
-        def __init__(self, target, target_component, action):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ACTION, 'ACTION')
-                self._fieldnames = ['target', 'target_component', 'action']
-                self.target = target
-                self.target_component = target_component
-                self.action = action
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 60, struct.pack('>BBB', self.target, self.target_component, self.action))
-
-class MAVLink_set_mode_message(MAVLink_message):
-        '''
-        Set the system mode, as defined by enum MAV_MODE in
-        mavlink/include/mavlink_types.h. There is no target component
-        id as the mode is by definition for the overall aircraft, not
-        only for one component.
-        '''
-        def __init__(self, target, mode):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_MODE, 'SET_MODE')
-                self._fieldnames = ['target', 'mode']
-                self.target = target
-                self.mode = mode
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 186, struct.pack('>BB', self.target, self.mode))
-
-class MAVLink_set_nav_mode_message(MAVLink_message):
-        '''
-        Set the system navigation mode, as defined by enum
-        MAV_NAV_MODE in mavlink/include/mavlink_types.h. The
-        navigation mode applies to the whole aircraft and thus all
-        components.
-        '''
-        def __init__(self, target, nav_mode):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_NAV_MODE, 'SET_NAV_MODE')
-                self._fieldnames = ['target', 'nav_mode']
-                self.target = target
-                self.nav_mode = nav_mode
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 10, struct.pack('>BB', self.target, self.nav_mode))
-
-class MAVLink_param_request_read_message(MAVLink_message):
-        '''
-        Request to read the onboard parameter with the param_id string
-        id. Onboard parameters are stored as key[const char*] ->
-        value[float]. This allows to send a parameter to any other
-        component (such as the GCS) without the need of previous
-        knowledge of possible parameter names. Thus the same GCS can
-        store different parameters for different autopilots. See also
-        http://qgroundcontrol.org/parameter_interface for a full
-        documentation of QGroundControl and IMU code.
-        '''
-        def __init__(self, target_system, target_component, param_id, param_index):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_READ, 'PARAM_REQUEST_READ')
-                self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_index']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.param_id = param_id
-                self.param_index = param_index
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 89, struct.pack('>BB15sh', self.target_system, self.target_component, self.param_id, self.param_index))
-
-class MAVLink_param_request_list_message(MAVLink_message):
-        '''
-        Request all parameters of this component. After his request,
-        all parameters are emitted.
-        '''
-        def __init__(self, target_system, target_component):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, 'PARAM_REQUEST_LIST')
-                self._fieldnames = ['target_system', 'target_component']
-                self.target_system = target_system
-                self.target_component = target_component
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 159, struct.pack('>BB', self.target_system, self.target_component))
-
-class MAVLink_param_value_message(MAVLink_message):
-        '''
-        Emit the value of a onboard parameter. The inclusion of
-        param_count and param_index in the message allows the
-        recipient to keep track of received parameters and allows him
-        to re-request missing parameters after a loss or timeout.
-        '''
-        def __init__(self, param_id, param_value, param_count, param_index):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_VALUE, 'PARAM_VALUE')
-                self._fieldnames = ['param_id', 'param_value', 'param_count', 'param_index']
-                self.param_id = param_id
-                self.param_value = param_value
-                self.param_count = param_count
-                self.param_index = param_index
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 162, struct.pack('>15sfHH', self.param_id, self.param_value, self.param_count, self.param_index))
-
-class MAVLink_param_set_message(MAVLink_message):
-        '''
-        Set a parameter value TEMPORARILY to RAM. It will be reset to
-        default on system reboot. Send the ACTION
-        MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents
-        to EEPROM. IMPORTANT: The receiving component should
-        acknowledge the new parameter value by sending a param_value
-        message to all communication partners. This will also ensure
-        that multiple GCS all have an up-to-date list of all
-        parameters. If the sending GCS did not receive a PARAM_VALUE
-        message within its timeout time, it should re-send the
-        PARAM_SET message.
-        '''
-        def __init__(self, target_system, target_component, param_id, param_value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_SET, 'PARAM_SET')
-                self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_value']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.param_id = param_id
-                self.param_value = param_value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 121, struct.pack('>BB15sf', self.target_system, self.target_component, self.param_id, self.param_value))
-
-class MAVLink_gps_raw_int_message(MAVLink_message):
-        '''
-        The global position, as returned by the Global Positioning
-        System (GPS). This is NOT the global position estimate of the
-        sytem, but rather a RAW sensor value. See message
-        GLOBAL_POSITION for the global position estimate. Coordinate
-        frame is right-handed, Z-axis up (GPS frame)
-        '''
-        def __init__(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_RAW_INT, 'GPS_RAW_INT')
-                self._fieldnames = ['usec', 'fix_type', 'lat', 'lon', 'alt', 'eph', 'epv', 'v', 'hdg']
-                self.usec = usec
-                self.fix_type = fix_type
-                self.lat = lat
-                self.lon = lon
-                self.alt = alt
-                self.eph = eph
-                self.epv = epv
-                self.v = v
-                self.hdg = hdg
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 149, struct.pack('>QBiiiffff', self.usec, self.fix_type, self.lat, self.lon, self.alt, self.eph, self.epv, self.v, self.hdg))
-
-class MAVLink_scaled_imu_message(MAVLink_message):
-        '''
-        The RAW IMU readings for the usual 9DOF sensor setup. This
-        message should contain the scaled values to the described
-        units
-        '''
-        def __init__(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SCALED_IMU, 'SCALED_IMU')
-                self._fieldnames = ['usec', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro', 'xmag', 'ymag', 'zmag']
-                self.usec = usec
-                self.xacc = xacc
-                self.yacc = yacc
-                self.zacc = zacc
-                self.xgyro = xgyro
-                self.ygyro = ygyro
-                self.zgyro = zgyro
-                self.xmag = xmag
-                self.ymag = ymag
-                self.zmag = zmag
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 222, struct.pack('>Qhhhhhhhhh', self.usec, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro, self.xmag, self.ymag, self.zmag))
-
-class MAVLink_gps_status_message(MAVLink_message):
-        '''
-        The positioning status, as reported by GPS. This message is
-        intended to display status information about each satellite
-        visible to the receiver. See message GLOBAL_POSITION for the
-        global position estimate. This message can contain information
-        for up to 20 satellites.
-        '''
-        def __init__(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_STATUS, 'GPS_STATUS')
-                self._fieldnames = ['satellites_visible', 'satellite_prn', 'satellite_used', 'satellite_elevation', 'satellite_azimuth', 'satellite_snr']
-                self.satellites_visible = satellites_visible
-                self.satellite_prn = satellite_prn
-                self.satellite_used = satellite_used
-                self.satellite_elevation = satellite_elevation
-                self.satellite_azimuth = satellite_azimuth
-                self.satellite_snr = satellite_snr
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 110, struct.pack('>B20s20s20s20s20s', self.satellites_visible, self.satellite_prn, self.satellite_used, self.satellite_elevation, self.satellite_azimuth, self.satellite_snr))
-
-class MAVLink_raw_imu_message(MAVLink_message):
-        '''
-        The RAW IMU readings for the usual 9DOF sensor setup. This
-        message should always contain the true raw values without any
-        scaling to allow data capture and system debugging.
-        '''
-        def __init__(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RAW_IMU, 'RAW_IMU')
-                self._fieldnames = ['usec', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro', 'xmag', 'ymag', 'zmag']
-                self.usec = usec
-                self.xacc = xacc
-                self.yacc = yacc
-                self.zacc = zacc
-                self.xgyro = xgyro
-                self.ygyro = ygyro
-                self.zgyro = zgyro
-                self.xmag = xmag
-                self.ymag = ymag
-                self.zmag = zmag
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 179, struct.pack('>Qhhhhhhhhh', self.usec, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro, self.xmag, self.ymag, self.zmag))
-
-class MAVLink_raw_pressure_message(MAVLink_message):
-        '''
-        The RAW pressure readings for the typical setup of one
-        absolute pressure and one differential pressure sensor. The
-        sensor values should be the raw, UNSCALED ADC values.
-        '''
-        def __init__(self, usec, press_abs, press_diff1, press_diff2, temperature):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RAW_PRESSURE, 'RAW_PRESSURE')
-                self._fieldnames = ['usec', 'press_abs', 'press_diff1', 'press_diff2', 'temperature']
-                self.usec = usec
-                self.press_abs = press_abs
-                self.press_diff1 = press_diff1
-                self.press_diff2 = press_diff2
-                self.temperature = temperature
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 136, struct.pack('>Qhhhh', self.usec, self.press_abs, self.press_diff1, self.press_diff2, self.temperature))
-
-class MAVLink_scaled_pressure_message(MAVLink_message):
-        '''
-        The pressure readings for the typical setup of one absolute
-        and differential pressure sensor. The units are as specified
-        in each field.
-        '''
-        def __init__(self, usec, press_abs, press_diff, temperature):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SCALED_PRESSURE, 'SCALED_PRESSURE')
-                self._fieldnames = ['usec', 'press_abs', 'press_diff', 'temperature']
-                self.usec = usec
-                self.press_abs = press_abs
-                self.press_diff = press_diff
-                self.temperature = temperature
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 229, struct.pack('>Qffh', self.usec, self.press_abs, self.press_diff, self.temperature))
-
-class MAVLink_attitude_message(MAVLink_message):
-        '''
-        The attitude in the aeronautical frame (right-handed, Z-down,
-        X-front, Y-right).
-        '''
-        def __init__(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ATTITUDE, 'ATTITUDE')
-                self._fieldnames = ['usec', 'roll', 'pitch', 'yaw', 'rollspeed', 'pitchspeed', 'yawspeed']
-                self.usec = usec
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.rollspeed = rollspeed
-                self.pitchspeed = pitchspeed
-                self.yawspeed = yawspeed
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 66, struct.pack('>Qffffff', self.usec, self.roll, self.pitch, self.yaw, self.rollspeed, self.pitchspeed, self.yawspeed))
-
-class MAVLink_local_position_message(MAVLink_message):
-        '''
-        The filtered local position (e.g. fused computer vision and
-        accelerometers). Coordinate frame is right-handed, Z-axis down
-        (aeronautical frame)
-        '''
-        def __init__(self, usec, x, y, z, vx, vy, vz):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION, 'LOCAL_POSITION')
-                self._fieldnames = ['usec', 'x', 'y', 'z', 'vx', 'vy', 'vz']
-                self.usec = usec
-                self.x = x
-                self.y = y
-                self.z = z
-                self.vx = vx
-                self.vy = vy
-                self.vz = vz
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 126, struct.pack('>Qffffff', self.usec, self.x, self.y, self.z, self.vx, self.vy, self.vz))
-
-class MAVLink_global_position_message(MAVLink_message):
-        '''
-        The filtered global position (e.g. fused GPS and
-        accelerometers). Coordinate frame is right-handed, Z-axis up
-        (GPS frame)
-        '''
-        def __init__(self, usec, lat, lon, alt, vx, vy, vz):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_POSITION, 'GLOBAL_POSITION')
-                self._fieldnames = ['usec', 'lat', 'lon', 'alt', 'vx', 'vy', 'vz']
-                self.usec = usec
-                self.lat = lat
-                self.lon = lon
-                self.alt = alt
-                self.vx = vx
-                self.vy = vy
-                self.vz = vz
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 147, struct.pack('>Qffffff', self.usec, self.lat, self.lon, self.alt, self.vx, self.vy, self.vz))
-
-class MAVLink_gps_raw_message(MAVLink_message):
-        '''
-        The global position, as returned by the Global Positioning
-        System (GPS). This is NOT the global position estimate of the
-        sytem, but rather a RAW sensor value. See message
-        GLOBAL_POSITION for the global position estimate. Coordinate
-        frame is right-handed, Z-axis up (GPS frame)
-        '''
-        def __init__(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_RAW, 'GPS_RAW')
-                self._fieldnames = ['usec', 'fix_type', 'lat', 'lon', 'alt', 'eph', 'epv', 'v', 'hdg']
-                self.usec = usec
-                self.fix_type = fix_type
-                self.lat = lat
-                self.lon = lon
-                self.alt = alt
-                self.eph = eph
-                self.epv = epv
-                self.v = v
-                self.hdg = hdg
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 185, struct.pack('>QBfffffff', self.usec, self.fix_type, self.lat, self.lon, self.alt, self.eph, self.epv, self.v, self.hdg))
-
-class MAVLink_sys_status_message(MAVLink_message):
-        '''
-        The general system state. If the system is following the
-        MAVLink standard, the system state is mainly defined by three
-        orthogonal states/modes: The system mode, which is either
-        LOCKED (motors shut down and locked), MANUAL (system under RC
-        control), GUIDED (system with autonomous position control,
-        position setpoint controlled manually) or AUTO (system guided
-        by path/waypoint planner). The NAV_MODE defined the current
-        flight state: LIFTOFF (often an open-loop maneuver), LANDING,
-        WAYPOINTS or VECTOR. This represents the internal navigation
-        state machine. The system status shows wether the system is
-        currently active or not and if an emergency occured. During
-        the CRITICAL and EMERGENCY states the MAV is still considered
-        to be active, but should start emergency procedures
-        autonomously. After a failure occured it should first move
-        from active to critical to allow manual intervention and then
-        move to emergency after a certain timeout.
-        '''
-        def __init__(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYS_STATUS, 'SYS_STATUS')
-                self._fieldnames = ['mode', 'nav_mode', 'status', 'load', 'vbat', 'battery_remaining', 'packet_drop']
-                self.mode = mode
-                self.nav_mode = nav_mode
-                self.status = status
-                self.load = load
-                self.vbat = vbat
-                self.battery_remaining = battery_remaining
-                self.packet_drop = packet_drop
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 112, struct.pack('>BBBHHHH', self.mode, self.nav_mode, self.status, self.load, self.vbat, self.battery_remaining, self.packet_drop))
-
-class MAVLink_rc_channels_raw_message(MAVLink_message):
-        '''
-        The RAW values of the RC channels received. The standard PPM
-        modulation is as follows: 1000 microseconds: 0%, 2000
-        microseconds: 100%. Individual receivers/transmitters might
-        violate this specification.
-        '''
-        def __init__(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_RAW, 'RC_CHANNELS_RAW')
-                self._fieldnames = ['chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw', 'rssi']
-                self.chan1_raw = chan1_raw
-                self.chan2_raw = chan2_raw
-                self.chan3_raw = chan3_raw
-                self.chan4_raw = chan4_raw
-                self.chan5_raw = chan5_raw
-                self.chan6_raw = chan6_raw
-                self.chan7_raw = chan7_raw
-                self.chan8_raw = chan8_raw
-                self.rssi = rssi
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 252, struct.pack('>HHHHHHHHB', self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw, self.rssi))
-
-class MAVLink_rc_channels_scaled_message(MAVLink_message):
-        '''
-        The scaled values of the RC channels received. (-100%) -10000,
-        (0%) 0, (100%) 10000
-        '''
-        def __init__(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, 'RC_CHANNELS_SCALED')
-                self._fieldnames = ['chan1_scaled', 'chan2_scaled', 'chan3_scaled', 'chan4_scaled', 'chan5_scaled', 'chan6_scaled', 'chan7_scaled', 'chan8_scaled', 'rssi']
-                self.chan1_scaled = chan1_scaled
-                self.chan2_scaled = chan2_scaled
-                self.chan3_scaled = chan3_scaled
-                self.chan4_scaled = chan4_scaled
-                self.chan5_scaled = chan5_scaled
-                self.chan6_scaled = chan6_scaled
-                self.chan7_scaled = chan7_scaled
-                self.chan8_scaled = chan8_scaled
-                self.rssi = rssi
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 162, struct.pack('>hhhhhhhhB', self.chan1_scaled, self.chan2_scaled, self.chan3_scaled, self.chan4_scaled, self.chan5_scaled, self.chan6_scaled, self.chan7_scaled, self.chan8_scaled, self.rssi))
-
-class MAVLink_servo_output_raw_message(MAVLink_message):
-        '''
-        The RAW values of the servo outputs (for RC input from the
-        remote, use the RC_CHANNELS messages). The standard PPM
-        modulation is as follows: 1000 microseconds: 0%, 2000
-        microseconds: 100%.
-        '''
-        def __init__(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 'SERVO_OUTPUT_RAW')
-                self._fieldnames = ['servo1_raw', 'servo2_raw', 'servo3_raw', 'servo4_raw', 'servo5_raw', 'servo6_raw', 'servo7_raw', 'servo8_raw']
-                self.servo1_raw = servo1_raw
-                self.servo2_raw = servo2_raw
-                self.servo3_raw = servo3_raw
-                self.servo4_raw = servo4_raw
-                self.servo5_raw = servo5_raw
-                self.servo6_raw = servo6_raw
-                self.servo7_raw = servo7_raw
-                self.servo8_raw = servo8_raw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 215, struct.pack('>HHHHHHHH', self.servo1_raw, self.servo2_raw, self.servo3_raw, self.servo4_raw, self.servo5_raw, self.servo6_raw, self.servo7_raw, self.servo8_raw))
-
-class MAVLink_waypoint_message(MAVLink_message):
-        '''
-        Message encoding a waypoint. This message is emitted to
-        announce      the presence of a waypoint and to set a waypoint
-        on the system. The waypoint can be either in x, y, z meters
-        (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is
-        Z-down, right handed, global frame is Z-up, right handed
-        '''
-        def __init__(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT, 'WAYPOINT')
-                self._fieldnames = ['target_system', 'target_component', 'seq', 'frame', 'command', 'current', 'autocontinue', 'param1', 'param2', 'param3', 'param4', 'x', 'y', 'z']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.seq = seq
-                self.frame = frame
-                self.command = command
-                self.current = current
-                self.autocontinue = autocontinue
-                self.param1 = param1
-                self.param2 = param2
-                self.param3 = param3
-                self.param4 = param4
-                self.x = x
-                self.y = y
-                self.z = z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 128, struct.pack('>BBHBBBBfffffff', self.target_system, self.target_component, self.seq, self.frame, self.command, self.current, self.autocontinue, self.param1, self.param2, self.param3, self.param4, self.x, self.y, self.z))
-
-class MAVLink_waypoint_request_message(MAVLink_message):
-        '''
-        Request the information of the waypoint with the sequence
-        number seq. The response of the system to this message should
-        be a WAYPOINT message.
-        '''
-        def __init__(self, target_system, target_component, seq):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REQUEST, 'WAYPOINT_REQUEST')
-                self._fieldnames = ['target_system', 'target_component', 'seq']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.seq = seq
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 9, struct.pack('>BBH', self.target_system, self.target_component, self.seq))
-
-class MAVLink_waypoint_set_current_message(MAVLink_message):
-        '''
-        Set the waypoint with sequence number seq as current waypoint.
-        This means that the MAV will continue to this waypoint on the
-        shortest path (not following the waypoints in-between).
-        '''
-        def __init__(self, target_system, target_component, seq):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, 'WAYPOINT_SET_CURRENT')
-                self._fieldnames = ['target_system', 'target_component', 'seq']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.seq = seq
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 106, struct.pack('>BBH', self.target_system, self.target_component, self.seq))
-
-class MAVLink_waypoint_current_message(MAVLink_message):
-        '''
-        Message that announces the sequence number of the current
-        active waypoint. The MAV will fly towards this waypoint.
-        '''
-        def __init__(self, seq):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_CURRENT, 'WAYPOINT_CURRENT')
-                self._fieldnames = ['seq']
-                self.seq = seq
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 101, struct.pack('>H', self.seq))
-
-class MAVLink_waypoint_request_list_message(MAVLink_message):
-        '''
-        Request the overall list of waypoints from the
-        system/component.
-        '''
-        def __init__(self, target_system, target_component):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, 'WAYPOINT_REQUEST_LIST')
-                self._fieldnames = ['target_system', 'target_component']
-                self.target_system = target_system
-                self.target_component = target_component
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 213, struct.pack('>BB', self.target_system, self.target_component))
-
-class MAVLink_waypoint_count_message(MAVLink_message):
-        '''
-        This message is emitted as response to WAYPOINT_REQUEST_LIST
-        by the MAV. The GCS can then request the individual waypoints
-        based on the knowledge of the total number of waypoints.
-        '''
-        def __init__(self, target_system, target_component, count):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_COUNT, 'WAYPOINT_COUNT')
-                self._fieldnames = ['target_system', 'target_component', 'count']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.count = count
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 4, struct.pack('>BBH', self.target_system, self.target_component, self.count))
-
-class MAVLink_waypoint_clear_all_message(MAVLink_message):
-        '''
-        Delete all waypoints at once.
-        '''
-        def __init__(self, target_system, target_component):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, 'WAYPOINT_CLEAR_ALL')
-                self._fieldnames = ['target_system', 'target_component']
-                self.target_system = target_system
-                self.target_component = target_component
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 229, struct.pack('>BB', self.target_system, self.target_component))
-
-class MAVLink_waypoint_reached_message(MAVLink_message):
-        '''
-        A certain waypoint has been reached. The system will either
-        hold this position (or circle on the orbit) or (if the
-        autocontinue on the WP was set) continue to the next waypoint.
-        '''
-        def __init__(self, seq):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REACHED, 'WAYPOINT_REACHED')
-                self._fieldnames = ['seq']
-                self.seq = seq
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 21, struct.pack('>H', self.seq))
-
-class MAVLink_waypoint_ack_message(MAVLink_message):
-        '''
-        Ack message during waypoint handling. The type field states if
-        this message is a positive ack (type=0) or if an error
-        happened (type=non-zero).
-        '''
-        def __init__(self, target_system, target_component, type):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_ACK, 'WAYPOINT_ACK')
-                self._fieldnames = ['target_system', 'target_component', 'type']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.type = type
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 214, struct.pack('>BBB', self.target_system, self.target_component, self.type))
-
-class MAVLink_gps_set_global_origin_message(MAVLink_message):
-        '''
-        As local waypoints exist, the global waypoint reference allows
-        to transform between the local coordinate frame and the global
-        (GPS) coordinate frame. This can be necessary when e.g. in-
-        and outdoor settings are connected and the MAV should move
-        from in- to outdoor.
-        '''
-        def __init__(self, target_system, target_component, latitude, longitude, altitude):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, 'GPS_SET_GLOBAL_ORIGIN')
-                self._fieldnames = ['target_system', 'target_component', 'latitude', 'longitude', 'altitude']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.latitude = latitude
-                self.longitude = longitude
-                self.altitude = altitude
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 215, struct.pack('>BBiii', self.target_system, self.target_component, self.latitude, self.longitude, self.altitude))
-
-class MAVLink_gps_local_origin_set_message(MAVLink_message):
-        '''
-        Once the MAV sets a new GPS-Local correspondence, this message
-        announces the origin (0,0,0) position
-        '''
-        def __init__(self, latitude, longitude, altitude):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET, 'GPS_LOCAL_ORIGIN_SET')
-                self._fieldnames = ['latitude', 'longitude', 'altitude']
-                self.latitude = latitude
-                self.longitude = longitude
-                self.altitude = altitude
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 14, struct.pack('>iii', self.latitude, self.longitude, self.altitude))
-
-class MAVLink_local_position_setpoint_set_message(MAVLink_message):
-        '''
-        Set the setpoint for a local position controller. This is the
-        position in local coordinates the MAV should fly to. This
-        message is sent by the path/waypoint planner to the onboard
-        position controller. As some MAVs have a degree of freedom in
-        yaw (e.g. all helicopters/quadrotors), the desired yaw angle
-        is part of the message.
-        '''
-        def __init__(self, target_system, target_component, x, y, z, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET, 'LOCAL_POSITION_SETPOINT_SET')
-                self._fieldnames = ['target_system', 'target_component', 'x', 'y', 'z', 'yaw']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.x = x
-                self.y = y
-                self.z = z
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 206, struct.pack('>BBffff', self.target_system, self.target_component, self.x, self.y, self.z, self.yaw))
-
-class MAVLink_local_position_setpoint_message(MAVLink_message):
-        '''
-        Transmit the current local setpoint of the controller to other
-        MAVs (collision avoidance) and to the GCS.
-        '''
-        def __init__(self, x, y, z, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, 'LOCAL_POSITION_SETPOINT')
-                self._fieldnames = ['x', 'y', 'z', 'yaw']
-                self.x = x
-                self.y = y
-                self.z = z
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 50, struct.pack('>ffff', self.x, self.y, self.z, self.yaw))
-
-class MAVLink_control_status_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_CONTROL_STATUS, 'CONTROL_STATUS')
-                self._fieldnames = ['position_fix', 'vision_fix', 'gps_fix', 'ahrs_health', 'control_att', 'control_pos_xy', 'control_pos_z', 'control_pos_yaw']
-                self.position_fix = position_fix
-                self.vision_fix = vision_fix
-                self.gps_fix = gps_fix
-                self.ahrs_health = ahrs_health
-                self.control_att = control_att
-                self.control_pos_xy = control_pos_xy
-                self.control_pos_z = control_pos_z
-                self.control_pos_yaw = control_pos_yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 157, struct.pack('>BBBBBBBB', self.position_fix, self.vision_fix, self.gps_fix, self.ahrs_health, self.control_att, self.control_pos_xy, self.control_pos_z, self.control_pos_yaw))
-
-class MAVLink_safety_set_allowed_area_message(MAVLink_message):
-        '''
-        Set a safety zone (volume), which is defined by two corners of
-        a cube. This message can be used to tell the MAV which
-        setpoints/waypoints to accept and which to reject. Safety
-        areas are often enforced by national or competition
-        regulations.
-        '''
-        def __init__(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, 'SAFETY_SET_ALLOWED_AREA')
-                self._fieldnames = ['target_system', 'target_component', 'frame', 'p1x', 'p1y', 'p1z', 'p2x', 'p2y', 'p2z']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.frame = frame
-                self.p1x = p1x
-                self.p1y = p1y
-                self.p1z = p1z
-                self.p2x = p2x
-                self.p2y = p2y
-                self.p2z = p2z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 126, struct.pack('>BBBffffff', self.target_system, self.target_component, self.frame, self.p1x, self.p1y, self.p1z, self.p2x, self.p2y, self.p2z))
-
-class MAVLink_safety_allowed_area_message(MAVLink_message):
-        '''
-        Read out the safety zone the MAV currently assumes.
-        '''
-        def __init__(self, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, 'SAFETY_ALLOWED_AREA')
-                self._fieldnames = ['frame', 'p1x', 'p1y', 'p1z', 'p2x', 'p2y', 'p2z']
-                self.frame = frame
-                self.p1x = p1x
-                self.p1y = p1y
-                self.p1z = p1z
-                self.p2x = p2x
-                self.p2y = p2y
-                self.p2z = p2z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 108, struct.pack('>Bffffff', self.frame, self.p1x, self.p1y, self.p1z, self.p2x, self.p2y, self.p2z))
-
-class MAVLink_set_roll_pitch_yaw_thrust_message(MAVLink_message):
-        '''
-        Set roll, pitch and yaw.
-        '''
-        def __init__(self, target_system, target_component, roll, pitch, yaw, thrust):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, 'SET_ROLL_PITCH_YAW_THRUST')
-                self._fieldnames = ['target_system', 'target_component', 'roll', 'pitch', 'yaw', 'thrust']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.thrust = thrust
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 213, struct.pack('>BBffff', self.target_system, self.target_component, self.roll, self.pitch, self.yaw, self.thrust))
-
-class MAVLink_set_roll_pitch_yaw_speed_thrust_message(MAVLink_message):
-        '''
-        Set roll, pitch and yaw.
-        '''
-        def __init__(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, 'SET_ROLL_PITCH_YAW_SPEED_THRUST')
-                self._fieldnames = ['target_system', 'target_component', 'roll_speed', 'pitch_speed', 'yaw_speed', 'thrust']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.roll_speed = roll_speed
-                self.pitch_speed = pitch_speed
-                self.yaw_speed = yaw_speed
-                self.thrust = thrust
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 95, struct.pack('>BBffff', self.target_system, self.target_component, self.roll_speed, self.pitch_speed, self.yaw_speed, self.thrust))
-
-class MAVLink_roll_pitch_yaw_thrust_setpoint_message(MAVLink_message):
-        '''
-        Setpoint in roll, pitch, yaw currently active on the system.
-        '''
-        def __init__(self, time_us, roll, pitch, yaw, thrust):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, 'ROLL_PITCH_YAW_THRUST_SETPOINT')
-                self._fieldnames = ['time_us', 'roll', 'pitch', 'yaw', 'thrust']
-                self.time_us = time_us
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.thrust = thrust
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 5, struct.pack('>Qffff', self.time_us, self.roll, self.pitch, self.yaw, self.thrust))
-
-class MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(MAVLink_message):
-        '''
-        Setpoint in rollspeed, pitchspeed, yawspeed currently active
-        on the system.
-        '''
-        def __init__(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, 'ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT')
-                self._fieldnames = ['time_us', 'roll_speed', 'pitch_speed', 'yaw_speed', 'thrust']
-                self.time_us = time_us
-                self.roll_speed = roll_speed
-                self.pitch_speed = pitch_speed
-                self.yaw_speed = yaw_speed
-                self.thrust = thrust
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 127, struct.pack('>Qffff', self.time_us, self.roll_speed, self.pitch_speed, self.yaw_speed, self.thrust))
-
-class MAVLink_nav_controller_output_message(MAVLink_message):
-        '''
-        Outputs of the APM navigation controller. The primary use of
-        this message is to check the response and signs of the
-        controller before actual flight and to assist with tuning
-        controller parameters
-        '''
-        def __init__(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, 'NAV_CONTROLLER_OUTPUT')
-                self._fieldnames = ['nav_roll', 'nav_pitch', 'nav_bearing', 'target_bearing', 'wp_dist', 'alt_error', 'aspd_error', 'xtrack_error']
-                self.nav_roll = nav_roll
-                self.nav_pitch = nav_pitch
-                self.nav_bearing = nav_bearing
-                self.target_bearing = target_bearing
-                self.wp_dist = wp_dist
-                self.alt_error = alt_error
-                self.aspd_error = aspd_error
-                self.xtrack_error = xtrack_error
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 57, struct.pack('>ffhhHfff', self.nav_roll, self.nav_pitch, self.nav_bearing, self.target_bearing, self.wp_dist, self.alt_error, self.aspd_error, self.xtrack_error))
-
-class MAVLink_position_target_message(MAVLink_message):
-        '''
-        The goal position of the system. This position is the input to
-        any navigation or path planning algorithm and does NOT
-        represent the current controller setpoint.
-        '''
-        def __init__(self, x, y, z, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_POSITION_TARGET, 'POSITION_TARGET')
-                self._fieldnames = ['x', 'y', 'z', 'yaw']
-                self.x = x
-                self.y = y
-                self.z = z
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 126, struct.pack('>ffff', self.x, self.y, self.z, self.yaw))
-
-class MAVLink_state_correction_message(MAVLink_message):
-        '''
-        Corrects the systems state by adding an error correction term
-        to the position and velocity, and by rotating the attitude by
-        a correction angle.
-        '''
-        def __init__(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_STATE_CORRECTION, 'STATE_CORRECTION')
-                self._fieldnames = ['xErr', 'yErr', 'zErr', 'rollErr', 'pitchErr', 'yawErr', 'vxErr', 'vyErr', 'vzErr']
-                self.xErr = xErr
-                self.yErr = yErr
-                self.zErr = zErr
-                self.rollErr = rollErr
-                self.pitchErr = pitchErr
-                self.yawErr = yawErr
-                self.vxErr = vxErr
-                self.vyErr = vyErr
-                self.vzErr = vzErr
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 130, struct.pack('>fffffffff', self.xErr, self.yErr, self.zErr, self.rollErr, self.pitchErr, self.yawErr, self.vxErr, self.vyErr, self.vzErr))
-
-class MAVLink_set_altitude_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, target, mode):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ALTITUDE, 'SET_ALTITUDE')
-                self._fieldnames = ['target', 'mode']
-                self.target = target
-                self.mode = mode
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 119, struct.pack('>BI', self.target, self.mode))
-
-class MAVLink_request_data_stream_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, target_system, target_component, req_stream_id, req_message_rate, start_stop):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, 'REQUEST_DATA_STREAM')
-                self._fieldnames = ['target_system', 'target_component', 'req_stream_id', 'req_message_rate', 'start_stop']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.req_stream_id = req_stream_id
-                self.req_message_rate = req_message_rate
-                self.start_stop = start_stop
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 193, struct.pack('>BBBHB', self.target_system, self.target_component, self.req_stream_id, self.req_message_rate, self.start_stop))
-
-class MAVLink_hil_state_message(MAVLink_message):
-        '''
-        This packet is useful for high throughput
-        applications such as hardware in the loop simulations.
-        '''
-        def __init__(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_STATE, 'HIL_STATE')
-                self._fieldnames = ['usec', 'roll', 'pitch', 'yaw', 'rollspeed', 'pitchspeed', 'yawspeed', 'lat', 'lon', 'alt', 'vx', 'vy', 'vz', 'xacc', 'yacc', 'zacc']
-                self.usec = usec
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.rollspeed = rollspeed
-                self.pitchspeed = pitchspeed
-                self.yawspeed = yawspeed
-                self.lat = lat
-                self.lon = lon
-                self.alt = alt
-                self.vx = vx
-                self.vy = vy
-                self.vz = vz
-                self.xacc = xacc
-                self.yacc = yacc
-                self.zacc = zacc
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 191, struct.pack('>Qffffffiiihhhhhh', self.usec, self.roll, self.pitch, self.yaw, self.rollspeed, self.pitchspeed, self.yawspeed, self.lat, self.lon, self.alt, self.vx, self.vy, self.vz, self.xacc, self.yacc, self.zacc))
-
-class MAVLink_hil_controls_message(MAVLink_message):
-        '''
-        Hardware in the loop control outputs
-        '''
-        def __init__(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_CONTROLS, 'HIL_CONTROLS')
-                self._fieldnames = ['time_us', 'roll_ailerons', 'pitch_elevator', 'yaw_rudder', 'throttle', 'mode', 'nav_mode']
-                self.time_us = time_us
-                self.roll_ailerons = roll_ailerons
-                self.pitch_elevator = pitch_elevator
-                self.yaw_rudder = yaw_rudder
-                self.throttle = throttle
-                self.mode = mode
-                self.nav_mode = nav_mode
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 236, struct.pack('>QffffBB', self.time_us, self.roll_ailerons, self.pitch_elevator, self.yaw_rudder, self.throttle, self.mode, self.nav_mode))
-
-class MAVLink_manual_control_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MANUAL_CONTROL, 'MANUAL_CONTROL')
-                self._fieldnames = ['target', 'roll', 'pitch', 'yaw', 'thrust', 'roll_manual', 'pitch_manual', 'yaw_manual', 'thrust_manual']
-                self.target = target
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.thrust = thrust
-                self.roll_manual = roll_manual
-                self.pitch_manual = pitch_manual
-                self.yaw_manual = yaw_manual
-                self.thrust_manual = thrust_manual
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 158, struct.pack('>BffffBBBB', self.target, self.roll, self.pitch, self.yaw, self.thrust, self.roll_manual, self.pitch_manual, self.yaw_manual, self.thrust_manual))
-
-class MAVLink_rc_channels_override_message(MAVLink_message):
-        '''
-        The RAW values of the RC channels sent to the MAV to override
-        info received from the RC radio. A value of -1 means no change
-        to that channel. A value of 0 means control of that channel
-        should be released back to the RC radio. The standard PPM
-        modulation is as follows: 1000 microseconds: 0%, 2000
-        microseconds: 100%. Individual receivers/transmitters might
-        violate this specification.
-        '''
-        def __init__(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, 'RC_CHANNELS_OVERRIDE')
-                self._fieldnames = ['target_system', 'target_component', 'chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.chan1_raw = chan1_raw
-                self.chan2_raw = chan2_raw
-                self.chan3_raw = chan3_raw
-                self.chan4_raw = chan4_raw
-                self.chan5_raw = chan5_raw
-                self.chan6_raw = chan6_raw
-                self.chan7_raw = chan7_raw
-                self.chan8_raw = chan8_raw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 143, struct.pack('>BBHHHHHHHH', self.target_system, self.target_component, self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw))
-
-class MAVLink_global_position_int_message(MAVLink_message):
-        '''
-        The filtered global position (e.g. fused GPS and
-        accelerometers). The position is in GPS-frame (right-handed,
-        Z-up)
-        '''
-        def __init__(self, lat, lon, alt, vx, vy, vz):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, 'GLOBAL_POSITION_INT')
-                self._fieldnames = ['lat', 'lon', 'alt', 'vx', 'vy', 'vz']
-                self.lat = lat
-                self.lon = lon
-                self.alt = alt
-                self.vx = vx
-                self.vy = vy
-                self.vz = vz
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 104, struct.pack('>iiihhh', self.lat, self.lon, self.alt, self.vx, self.vy, self.vz))
-
-class MAVLink_vfr_hud_message(MAVLink_message):
-        '''
-        Metrics typically displayed on a HUD for fixed wing aircraft
-        '''
-        def __init__(self, airspeed, groundspeed, heading, throttle, alt, climb):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_VFR_HUD, 'VFR_HUD')
-                self._fieldnames = ['airspeed', 'groundspeed', 'heading', 'throttle', 'alt', 'climb']
-                self.airspeed = airspeed
-                self.groundspeed = groundspeed
-                self.heading = heading
-                self.throttle = throttle
-                self.alt = alt
-                self.climb = climb
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 123, struct.pack('>ffhHff', self.airspeed, self.groundspeed, self.heading, self.throttle, self.alt, self.climb))
-
-class MAVLink_command_message(MAVLink_message):
-        '''
-        Send a command with up to four parameters to the MAV
-        '''
-        def __init__(self, target_system, target_component, command, confirmation, param1, param2, param3, param4):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_COMMAND, 'COMMAND')
-                self._fieldnames = ['target_system', 'target_component', 'command', 'confirmation', 'param1', 'param2', 'param3', 'param4']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.command = command
-                self.confirmation = confirmation
-                self.param1 = param1
-                self.param2 = param2
-                self.param3 = param3
-                self.param4 = param4
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 131, struct.pack('>BBBBffff', self.target_system, self.target_component, self.command, self.confirmation, self.param1, self.param2, self.param3, self.param4))
-
-class MAVLink_command_ack_message(MAVLink_message):
-        '''
-        Report status of a command. Includes feedback wether the
-        command was executed
-        '''
-        def __init__(self, command, result):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_COMMAND_ACK, 'COMMAND_ACK')
-                self._fieldnames = ['command', 'result']
-                self.command = command
-                self.result = result
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 8, struct.pack('>ff', self.command, self.result))
-
-class MAVLink_optical_flow_message(MAVLink_message):
-        '''
-        Optical flow from a flow sensor (e.g. optical mouse sensor)
-        '''
-        def __init__(self, time, sensor_id, flow_x, flow_y, quality, ground_distance):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_OPTICAL_FLOW, 'OPTICAL_FLOW')
-                self._fieldnames = ['time', 'sensor_id', 'flow_x', 'flow_y', 'quality', 'ground_distance']
-                self.time = time
-                self.sensor_id = sensor_id
-                self.flow_x = flow_x
-                self.flow_y = flow_y
-                self.quality = quality
-                self.ground_distance = ground_distance
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 174, struct.pack('>QBhhBf', self.time, self.sensor_id, self.flow_x, self.flow_y, self.quality, self.ground_distance))
-
-class MAVLink_object_detection_event_message(MAVLink_message):
-        '''
-        Object has been detected
-        '''
-        def __init__(self, time, object_id, type, name, quality, bearing, distance):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT, 'OBJECT_DETECTION_EVENT')
-                self._fieldnames = ['time', 'object_id', 'type', 'name', 'quality', 'bearing', 'distance']
-                self.time = time
-                self.object_id = object_id
-                self.type = type
-                self.name = name
-                self.quality = quality
-                self.bearing = bearing
-                self.distance = distance
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 155, struct.pack('>IHB20sBff', self.time, self.object_id, self.type, self.name, self.quality, self.bearing, self.distance))
-
-class MAVLink_debug_vect_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, name, usec, x, y, z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DEBUG_VECT, 'DEBUG_VECT')
-                self._fieldnames = ['name', 'usec', 'x', 'y', 'z']
-                self.name = name
-                self.usec = usec
-                self.x = x
-                self.y = y
-                self.z = z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 178, struct.pack('>10sQfff', self.name, self.usec, self.x, self.y, self.z))
-
-class MAVLink_named_value_float_message(MAVLink_message):
-        '''
-        Send a key-value pair as float. The use of this message is
-        discouraged for normal packets, but a quite efficient way for
-        testing new messages and getting experimental debug output.
-        '''
-        def __init__(self, name, value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 'NAMED_VALUE_FLOAT')
-                self._fieldnames = ['name', 'value']
-                self.name = name
-                self.value = value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 224, struct.pack('>10sf', self.name, self.value))
-
-class MAVLink_named_value_int_message(MAVLink_message):
-        '''
-        Send a key-value pair as integer. The use of this message is
-        discouraged for normal packets, but a quite efficient way for
-        testing new messages and getting experimental debug output.
-        '''
-        def __init__(self, name, value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAMED_VALUE_INT, 'NAMED_VALUE_INT')
-                self._fieldnames = ['name', 'value']
-                self.name = name
-                self.value = value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 60, struct.pack('>10si', self.name, self.value))
-
-class MAVLink_statustext_message(MAVLink_message):
-        '''
-        Status text message. These messages are printed in yellow in
-        the COMM console of QGroundControl. WARNING: They consume
-        quite some bandwidth, so use only for important status and
-        error messages. If implemented wisely, these messages are
-        buffered on the MCU and sent only at a limited rate (e.g. 10
-        Hz).
-        '''
-        def __init__(self, severity, text):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_STATUSTEXT, 'STATUSTEXT')
-                self._fieldnames = ['severity', 'text']
-                self.severity = severity
-                self.text = text
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 106, struct.pack('>B50s', self.severity, self.text))
-
-class MAVLink_debug_message(MAVLink_message):
-        '''
-        Send a debug value. The index is used to discriminate between
-        values. These values show up in the plot of QGroundControl as
-        DEBUG N.
-        '''
-        def __init__(self, ind, value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DEBUG, 'DEBUG')
-                self._fieldnames = ['ind', 'value']
-                self.ind = ind
-                self.value = value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 7, struct.pack('>Bf', self.ind, self.value))
-
-
-mavlink_map = {
-        MAVLINK_MSG_ID_SENSOR_OFFSETS : ( '>hhhfiiffffff', MAVLink_sensor_offsets_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11], 143 ),
-        MAVLINK_MSG_ID_SET_MAG_OFFSETS : ( '>BBhhh', MAVLink_set_mag_offsets_message, [0, 1, 2, 3, 4], 29 ),
-        MAVLINK_MSG_ID_MEMINFO : ( '>HH', MAVLink_meminfo_message, [0, 1], 208 ),
-        MAVLINK_MSG_ID_AP_ADC : ( '>HHHHHH', MAVLink_ap_adc_message, [0, 1, 2, 3, 4, 5], 188 ),
-        MAVLINK_MSG_ID_DIGICAM_CONFIGURE : ( '>BBBHBBBBBBf', MAVLink_digicam_configure_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10], 118 ),
-        MAVLINK_MSG_ID_DIGICAM_CONTROL : ( '>BBBBbBBBBf', MAVLink_digicam_control_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 242 ),
-        MAVLINK_MSG_ID_MOUNT_CONFIGURE : ( '>BBBBBB', MAVLink_mount_configure_message, [0, 1, 2, 3, 4, 5], 19 ),
-        MAVLINK_MSG_ID_MOUNT_CONTROL : ( '>BBiiiB', MAVLink_mount_control_message, [0, 1, 2, 3, 4, 5], 97 ),
-        MAVLINK_MSG_ID_MOUNT_STATUS : ( '>BBiii', MAVLink_mount_status_message, [0, 1, 2, 3, 4], 233 ),
-        MAVLINK_MSG_ID_FENCE_POINT : ( '>BBBBff', MAVLink_fence_point_message, [0, 1, 2, 3, 4, 5], 18 ),
-        MAVLINK_MSG_ID_FENCE_FETCH_POINT : ( '>BBB', MAVLink_fence_fetch_point_message, [0, 1, 2], 68 ),
-        MAVLINK_MSG_ID_FENCE_STATUS : ( '>BHBI', MAVLink_fence_status_message, [0, 1, 2, 3], 136 ),
-        MAVLINK_MSG_ID_AHRS : ( '>fffffff', MAVLink_ahrs_message, [0, 1, 2, 3, 4, 5, 6], 127 ),
-        MAVLINK_MSG_ID_SIMSTATE : ( '>fffffffff', MAVLink_simstate_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 42 ),
-        MAVLINK_MSG_ID_HWSTATUS : ( '>HB', MAVLink_hwstatus_message, [0, 1], 21 ),
-        MAVLINK_MSG_ID_RADIO : ( '>BBBBBHH', MAVLink_radio_message, [0, 1, 2, 3, 4, 5, 6], 93 ),
-        MAVLINK_MSG_ID_HEARTBEAT : ( '>BBB', MAVLink_heartbeat_message, [0, 1, 2], 72 ),
-        MAVLINK_MSG_ID_BOOT : ( '>I', MAVLink_boot_message, [0], 39 ),
-        MAVLINK_MSG_ID_SYSTEM_TIME : ( '>Q', MAVLink_system_time_message, [0], 190 ),
-        MAVLINK_MSG_ID_PING : ( '>IBBQ', MAVLink_ping_message, [0, 1, 2, 3], 92 ),
-        MAVLINK_MSG_ID_SYSTEM_TIME_UTC : ( '>II', MAVLink_system_time_utc_message, [0, 1], 191 ),
-        MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL : ( '>BBB25s', MAVLink_change_operator_control_message, [0, 1, 2, 3], 217 ),
-        MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK : ( '>BBB', MAVLink_change_operator_control_ack_message, [0, 1, 2], 104 ),
-        MAVLINK_MSG_ID_AUTH_KEY : ( '>32s', MAVLink_auth_key_message, [0], 119 ),
-        MAVLINK_MSG_ID_ACTION_ACK : ( '>BB', MAVLink_action_ack_message, [0, 1], 219 ),
-        MAVLINK_MSG_ID_ACTION : ( '>BBB', MAVLink_action_message, [0, 1, 2], 60 ),
-        MAVLINK_MSG_ID_SET_MODE : ( '>BB', MAVLink_set_mode_message, [0, 1], 186 ),
-        MAVLINK_MSG_ID_SET_NAV_MODE : ( '>BB', MAVLink_set_nav_mode_message, [0, 1], 10 ),
-        MAVLINK_MSG_ID_PARAM_REQUEST_READ : ( '>BB15sh', MAVLink_param_request_read_message, [0, 1, 2, 3], 89 ),
-        MAVLINK_MSG_ID_PARAM_REQUEST_LIST : ( '>BB', MAVLink_param_request_list_message, [0, 1], 159 ),
-        MAVLINK_MSG_ID_PARAM_VALUE : ( '>15sfHH', MAVLink_param_value_message, [0, 1, 2, 3], 162 ),
-        MAVLINK_MSG_ID_PARAM_SET : ( '>BB15sf', MAVLink_param_set_message, [0, 1, 2, 3], 121 ),
-        MAVLINK_MSG_ID_GPS_RAW_INT : ( '>QBiiiffff', MAVLink_gps_raw_int_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 149 ),
-        MAVLINK_MSG_ID_SCALED_IMU : ( '>Qhhhhhhhhh', MAVLink_scaled_imu_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 222 ),
-        MAVLINK_MSG_ID_GPS_STATUS : ( '>B20s20s20s20s20s', MAVLink_gps_status_message, [0, 1, 2, 3, 4, 5], 110 ),
-        MAVLINK_MSG_ID_RAW_IMU : ( '>Qhhhhhhhhh', MAVLink_raw_imu_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 179 ),
-        MAVLINK_MSG_ID_RAW_PRESSURE : ( '>Qhhhh', MAVLink_raw_pressure_message, [0, 1, 2, 3, 4], 136 ),
-        MAVLINK_MSG_ID_SCALED_PRESSURE : ( '>Qffh', MAVLink_scaled_pressure_message, [0, 1, 2, 3], 229 ),
-        MAVLINK_MSG_ID_ATTITUDE : ( '>Qffffff', MAVLink_attitude_message, [0, 1, 2, 3, 4, 5, 6], 66 ),
-        MAVLINK_MSG_ID_LOCAL_POSITION : ( '>Qffffff', MAVLink_local_position_message, [0, 1, 2, 3, 4, 5, 6], 126 ),
-        MAVLINK_MSG_ID_GLOBAL_POSITION : ( '>Qffffff', MAVLink_global_position_message, [0, 1, 2, 3, 4, 5, 6], 147 ),
-        MAVLINK_MSG_ID_GPS_RAW : ( '>QBfffffff', MAVLink_gps_raw_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 185 ),
-        MAVLINK_MSG_ID_SYS_STATUS : ( '>BBBHHHH', MAVLink_sys_status_message, [0, 1, 2, 3, 4, 5, 6], 112 ),
-        MAVLINK_MSG_ID_RC_CHANNELS_RAW : ( '>HHHHHHHHB', MAVLink_rc_channels_raw_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 252 ),
-        MAVLINK_MSG_ID_RC_CHANNELS_SCALED : ( '>hhhhhhhhB', MAVLink_rc_channels_scaled_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 162 ),
-        MAVLINK_MSG_ID_SERVO_OUTPUT_RAW : ( '>HHHHHHHH', MAVLink_servo_output_raw_message, [0, 1, 2, 3, 4, 5, 6, 7], 215 ),
-        MAVLINK_MSG_ID_WAYPOINT : ( '>BBHBBBBfffffff', MAVLink_waypoint_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13], 128 ),
-        MAVLINK_MSG_ID_WAYPOINT_REQUEST : ( '>BBH', MAVLink_waypoint_request_message, [0, 1, 2], 9 ),
-        MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT : ( '>BBH', MAVLink_waypoint_set_current_message, [0, 1, 2], 106 ),
-        MAVLINK_MSG_ID_WAYPOINT_CURRENT : ( '>H', MAVLink_waypoint_current_message, [0], 101 ),
-        MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST : ( '>BB', MAVLink_waypoint_request_list_message, [0, 1], 213 ),
-        MAVLINK_MSG_ID_WAYPOINT_COUNT : ( '>BBH', MAVLink_waypoint_count_message, [0, 1, 2], 4 ),
-        MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL : ( '>BB', MAVLink_waypoint_clear_all_message, [0, 1], 229 ),
-        MAVLINK_MSG_ID_WAYPOINT_REACHED : ( '>H', MAVLink_waypoint_reached_message, [0], 21 ),
-        MAVLINK_MSG_ID_WAYPOINT_ACK : ( '>BBB', MAVLink_waypoint_ack_message, [0, 1, 2], 214 ),
-        MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN : ( '>BBiii', MAVLink_gps_set_global_origin_message, [0, 1, 2, 3, 4], 215 ),
-        MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET : ( '>iii', MAVLink_gps_local_origin_set_message, [0, 1, 2], 14 ),
-        MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET : ( '>BBffff', MAVLink_local_position_setpoint_set_message, [0, 1, 2, 3, 4, 5], 206 ),
-        MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT : ( '>ffff', MAVLink_local_position_setpoint_message, [0, 1, 2, 3], 50 ),
-        MAVLINK_MSG_ID_CONTROL_STATUS : ( '>BBBBBBBB', MAVLink_control_status_message, [0, 1, 2, 3, 4, 5, 6, 7], 157 ),
-        MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA : ( '>BBBffffff', MAVLink_safety_set_allowed_area_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 126 ),
-        MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA : ( '>Bffffff', MAVLink_safety_allowed_area_message, [0, 1, 2, 3, 4, 5, 6], 108 ),
-        MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST : ( '>BBffff', MAVLink_set_roll_pitch_yaw_thrust_message, [0, 1, 2, 3, 4, 5], 213 ),
-        MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST : ( '>BBffff', MAVLink_set_roll_pitch_yaw_speed_thrust_message, [0, 1, 2, 3, 4, 5], 95 ),
-        MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT : ( '>Qffff', MAVLink_roll_pitch_yaw_thrust_setpoint_message, [0, 1, 2, 3, 4], 5 ),
-        MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT : ( '>Qffff', MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message, [0, 1, 2, 3, 4], 127 ),
-        MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT : ( '>ffhhHfff', MAVLink_nav_controller_output_message, [0, 1, 2, 3, 4, 5, 6, 7], 57 ),
-        MAVLINK_MSG_ID_POSITION_TARGET : ( '>ffff', MAVLink_position_target_message, [0, 1, 2, 3], 126 ),
-        MAVLINK_MSG_ID_STATE_CORRECTION : ( '>fffffffff', MAVLink_state_correction_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 130 ),
-        MAVLINK_MSG_ID_SET_ALTITUDE : ( '>BI', MAVLink_set_altitude_message, [0, 1], 119 ),
-        MAVLINK_MSG_ID_REQUEST_DATA_STREAM : ( '>BBBHB', MAVLink_request_data_stream_message, [0, 1, 2, 3, 4], 193 ),
-        MAVLINK_MSG_ID_HIL_STATE : ( '>Qffffffiiihhhhhh', MAVLink_hil_state_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15], 191 ),
-        MAVLINK_MSG_ID_HIL_CONTROLS : ( '>QffffBB', MAVLink_hil_controls_message, [0, 1, 2, 3, 4, 5, 6], 236 ),
-        MAVLINK_MSG_ID_MANUAL_CONTROL : ( '>BffffBBBB', MAVLink_manual_control_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 158 ),
-        MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE : ( '>BBHHHHHHHH', MAVLink_rc_channels_override_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 143 ),
-        MAVLINK_MSG_ID_GLOBAL_POSITION_INT : ( '>iiihhh', MAVLink_global_position_int_message, [0, 1, 2, 3, 4, 5], 104 ),
-        MAVLINK_MSG_ID_VFR_HUD : ( '>ffhHff', MAVLink_vfr_hud_message, [0, 1, 2, 3, 4, 5], 123 ),
-        MAVLINK_MSG_ID_COMMAND : ( '>BBBBffff', MAVLink_command_message, [0, 1, 2, 3, 4, 5, 6, 7], 131 ),
-        MAVLINK_MSG_ID_COMMAND_ACK : ( '>ff', MAVLink_command_ack_message, [0, 1], 8 ),
-        MAVLINK_MSG_ID_OPTICAL_FLOW : ( '>QBhhBf', MAVLink_optical_flow_message, [0, 1, 2, 3, 4, 5], 174 ),
-        MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT : ( '>IHB20sBff', MAVLink_object_detection_event_message, [0, 1, 2, 3, 4, 5, 6], 155 ),
-        MAVLINK_MSG_ID_DEBUG_VECT : ( '>10sQfff', MAVLink_debug_vect_message, [0, 1, 2, 3, 4], 178 ),
-        MAVLINK_MSG_ID_NAMED_VALUE_FLOAT : ( '>10sf', MAVLink_named_value_float_message, [0, 1], 224 ),
-        MAVLINK_MSG_ID_NAMED_VALUE_INT : ( '>10si', MAVLink_named_value_int_message, [0, 1], 60 ),
-        MAVLINK_MSG_ID_STATUSTEXT : ( '>B50s', MAVLink_statustext_message, [0, 1], 106 ),
-        MAVLINK_MSG_ID_DEBUG : ( '>Bf', MAVLink_debug_message, [0, 1], 7 ),
-}
-
-class MAVError(Exception):
-        '''MAVLink error class'''
-        def __init__(self, msg):
-            Exception.__init__(self, msg)
-            self.message = msg
-
-class MAVString(str):
-        '''NUL terminated string'''
-        def __init__(self, s):
-                str.__init__(self)
-        def __str__(self):
-            i = self.find(chr(0))
-            if i == -1:
-                return self[:]
-            return self[0:i]
-
-class MAVLink_bad_data(MAVLink_message):
-        '''
-        a piece of bad data in a mavlink stream
-        '''
-        def __init__(self, data, reason):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_BAD_DATA, 'BAD_DATA')
-                self._fieldnames = ['data', 'reason']
-                self.data = data
-                self.reason = reason
-                self._msgbuf = data
-            
-class MAVLink(object):
-        '''MAVLink protocol handling class'''
-        def __init__(self, file, srcSystem=0, srcComponent=0):
-                self.seq = 0
-                self.file = file
-                self.srcSystem = srcSystem
-                self.srcComponent = srcComponent
-                self.callback = None
-                self.callback_args = None
-                self.callback_kwargs = None
-                self.buf = array.array('B')
-                self.expected_length = 6
-                self.have_prefix_error = False
-                self.robust_parsing = False
-                self.protocol_marker = 85
-                self.little_endian = False
-                self.crc_extra = False
-                self.sort_fields = False
-                self.total_packets_sent = 0
-                self.total_bytes_sent = 0
-                self.total_packets_received = 0
-                self.total_bytes_received = 0
-                self.total_receive_errors = 0
-                self.startup_time = time.time()
-
-        def set_callback(self, callback, *args, **kwargs):
-            self.callback = callback
-            self.callback_args = args
-            self.callback_kwargs = kwargs
-            
-        def send(self, mavmsg):
-                '''send a MAVLink message'''
-                buf = mavmsg.pack(self)
-                self.file.write(buf)
-                self.seq = (self.seq + 1) % 255
-                self.total_packets_sent += 1
-                self.total_bytes_sent += len(buf)
-
-        def bytes_needed(self):
-            '''return number of bytes needed for next parsing stage'''
-            ret = self.expected_length - len(self.buf)
-            if ret <= 0:
-                return 1
-            return ret
-
-        def parse_char(self, c):
-            '''input some data bytes, possibly returning a new message'''
-            if isinstance(c, str):
-                self.buf.fromstring(c)
-            else:
-                self.buf.extend(c)
-            self.total_bytes_received += len(c)
-            if len(self.buf) >= 1 and self.buf[0] != 85:
-                magic = self.buf[0]
-                self.buf = self.buf[1:]
-                if self.robust_parsing:
-                    m = MAVLink_bad_data(chr(magic), "Bad prefix")
-                    if self.callback:
-                        self.callback(m, *self.callback_args, **self.callback_kwargs)
-                    self.expected_length = 6
-                    self.total_receive_errors += 1
-                    return m
-                if self.have_prefix_error:
-                    return None
-                self.have_prefix_error = True
-                self.total_receive_errors += 1
-                raise MAVError("invalid MAVLink prefix '%s'" % magic) 
-            self.have_prefix_error = False
-            if len(self.buf) >= 2:
-                (magic, self.expected_length) = struct.unpack('BB', self.buf[0:2])
-                self.expected_length += 8
-            if self.expected_length >= 8 and len(self.buf) >= self.expected_length:
-                mbuf = self.buf[0:self.expected_length]
-                self.buf = self.buf[self.expected_length:]
-                self.expected_length = 6
-                if self.robust_parsing:
-                    try:
-                        m = self.decode(mbuf)
-                        self.total_packets_received += 1
-                    except MAVError as reason:
-                        m = MAVLink_bad_data(mbuf, reason.message)
-                        self.total_receive_errors += 1
-                else:
-                    m = self.decode(mbuf)
-                    self.total_packets_received += 1
-                if self.callback:
-                    self.callback(m, *self.callback_args, **self.callback_kwargs)
-                return m
-            return None
-
-        def parse_buffer(self, s):
-            '''input some data bytes, possibly returning a list of new messages'''
-            m = self.parse_char(s)
-            if m is None:
-                return None
-            ret = [m]
-            while True:
-                m = self.parse_char("")
-                if m is None:
-                    return ret
-                ret.append(m)
-            return ret
-
-        def decode(self, msgbuf):
-                '''decode a buffer as a MAVLink message'''
-                # decode the header
-                try:
-                    magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
-                if ord(magic) != 85:
-                    raise MAVError("invalid MAVLink prefix '%s'" % magic)
-                if mlen != len(msgbuf)-8:
-                    raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf)-8, mlen, msgId))
-
-                if not msgId in mavlink_map:
-                    raise MAVError('unknown MAVLink message ID %u' % msgId)
-
-                # decode the payload
-                (fmt, type, order_map, crc_extra) = mavlink_map[msgId]
-
-                # decode the checksum
-                try:
-                    crc, = struct.unpack('<H', msgbuf[-2:])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink CRC: %s' % emsg)
-                crc2 = mavutil.x25crc(msgbuf[1:-2])
-                if False: # using CRC extra 
-                    crc2.accumulate(chr(crc_extra))
-                if crc != crc2.crc:
-                    raise MAVError('invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' % (msgId, crc, crc2.crc))
-
-                try:
-                    t = struct.unpack(fmt, msgbuf[6:-2])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink payload type=%s fmt=%s payloadLength=%u: %s' % (
-                        type, fmt, len(msgbuf[6:-2]), emsg))
-
-                tlist = list(t)
-                # handle sorted fields
-                if False:
-                    t = tlist[:]
-                    for i in range(0, len(tlist)):
-                        tlist[i] = t[order_map[i]]
-
-                # terminate any strings
-                for i in range(0, len(tlist)):
-                    if isinstance(tlist[i], str):
-                        tlist[i] = MAVString(tlist[i])
-                t = tuple(tlist)
-                # construct the message object
-                try:
-                    m = type(*t)
-                except Exception as emsg:
-                    raise MAVError('Unable to instantiate MAVLink message of type %s : %s' % (type, emsg))
-                m._msgbuf = msgbuf
-                m._payload = msgbuf[6:-2]
-                m._crc = crc
-                m._header = MAVLink_header(msgId, mlen, seq, srcSystem, srcComponent)
-                return m
-        def sensor_offsets_encode(self, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z):
-                '''
-                Offsets and calibrations values for hardware         sensors. This
-                makes it easier to debug the calibration process.
-
-                mag_ofs_x                 : magnetometer X offset (int16_t)
-                mag_ofs_y                 : magnetometer Y offset (int16_t)
-                mag_ofs_z                 : magnetometer Z offset (int16_t)
-                mag_declination           : magnetic declination (radians) (float)
-                raw_press                 : raw pressure from barometer (int32_t)
-                raw_temp                  : raw temperature from barometer (int32_t)
-                gyro_cal_x                : gyro X calibration (float)
-                gyro_cal_y                : gyro Y calibration (float)
-                gyro_cal_z                : gyro Z calibration (float)
-                accel_cal_x               : accel X calibration (float)
-                accel_cal_y               : accel Y calibration (float)
-                accel_cal_z               : accel Z calibration (float)
-
-                '''
-                msg = MAVLink_sensor_offsets_message(mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z)
-                msg.pack(self)
-                return msg
-            
-        def sensor_offsets_send(self, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z):
-                '''
-                Offsets and calibrations values for hardware         sensors. This
-                makes it easier to debug the calibration process.
-
-                mag_ofs_x                 : magnetometer X offset (int16_t)
-                mag_ofs_y                 : magnetometer Y offset (int16_t)
-                mag_ofs_z                 : magnetometer Z offset (int16_t)
-                mag_declination           : magnetic declination (radians) (float)
-                raw_press                 : raw pressure from barometer (int32_t)
-                raw_temp                  : raw temperature from barometer (int32_t)
-                gyro_cal_x                : gyro X calibration (float)
-                gyro_cal_y                : gyro Y calibration (float)
-                gyro_cal_z                : gyro Z calibration (float)
-                accel_cal_x               : accel X calibration (float)
-                accel_cal_y               : accel Y calibration (float)
-                accel_cal_z               : accel Z calibration (float)
-
-                '''
-                return self.send(self.sensor_offsets_encode(mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z))
-            
-        def set_mag_offsets_encode(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z):
-                '''
-                set the magnetometer offsets
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mag_ofs_x                 : magnetometer X offset (int16_t)
-                mag_ofs_y                 : magnetometer Y offset (int16_t)
-                mag_ofs_z                 : magnetometer Z offset (int16_t)
-
-                '''
-                msg = MAVLink_set_mag_offsets_message(target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z)
-                msg.pack(self)
-                return msg
-            
-        def set_mag_offsets_send(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z):
-                '''
-                set the magnetometer offsets
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mag_ofs_x                 : magnetometer X offset (int16_t)
-                mag_ofs_y                 : magnetometer Y offset (int16_t)
-                mag_ofs_z                 : magnetometer Z offset (int16_t)
-
-                '''
-                return self.send(self.set_mag_offsets_encode(target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z))
-            
-        def meminfo_encode(self, brkval, freemem):
-                '''
-                state of APM memory
-
-                brkval                    : heap top (uint16_t)
-                freemem                   : free memory (uint16_t)
-
-                '''
-                msg = MAVLink_meminfo_message(brkval, freemem)
-                msg.pack(self)
-                return msg
-            
-        def meminfo_send(self, brkval, freemem):
-                '''
-                state of APM memory
-
-                brkval                    : heap top (uint16_t)
-                freemem                   : free memory (uint16_t)
-
-                '''
-                return self.send(self.meminfo_encode(brkval, freemem))
-            
-        def ap_adc_encode(self, adc1, adc2, adc3, adc4, adc5, adc6):
-                '''
-                raw ADC output
-
-                adc1                      : ADC output 1 (uint16_t)
-                adc2                      : ADC output 2 (uint16_t)
-                adc3                      : ADC output 3 (uint16_t)
-                adc4                      : ADC output 4 (uint16_t)
-                adc5                      : ADC output 5 (uint16_t)
-                adc6                      : ADC output 6 (uint16_t)
-
-                '''
-                msg = MAVLink_ap_adc_message(adc1, adc2, adc3, adc4, adc5, adc6)
-                msg.pack(self)
-                return msg
-            
-        def ap_adc_send(self, adc1, adc2, adc3, adc4, adc5, adc6):
-                '''
-                raw ADC output
-
-                adc1                      : ADC output 1 (uint16_t)
-                adc2                      : ADC output 2 (uint16_t)
-                adc3                      : ADC output 3 (uint16_t)
-                adc4                      : ADC output 4 (uint16_t)
-                adc5                      : ADC output 5 (uint16_t)
-                adc6                      : ADC output 6 (uint16_t)
-
-                '''
-                return self.send(self.ap_adc_encode(adc1, adc2, adc3, adc4, adc5, adc6))
-            
-        def digicam_configure_encode(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value):
-                '''
-                Configure on-board Camera Control System.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mode                      : Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) (uint8_t)
-                shutter_speed             : Divisor number //e.g. 1000 means 1/1000 (0 means ignore) (uint16_t)
-                aperture                  : F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) (uint8_t)
-                iso                       : ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) (uint8_t)
-                exposure_type             : Exposure type enumeration from 1 to N (0 means ignore) (uint8_t)
-                command_id                : Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once (uint8_t)
-                engine_cut_off            : Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) (uint8_t)
-                extra_param               : Extra parameters enumeration (0 means ignore) (uint8_t)
-                extra_value               : Correspondent value to given extra_param (float)
-
-                '''
-                msg = MAVLink_digicam_configure_message(target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value)
-                msg.pack(self)
-                return msg
-            
-        def digicam_configure_send(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value):
-                '''
-                Configure on-board Camera Control System.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mode                      : Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) (uint8_t)
-                shutter_speed             : Divisor number //e.g. 1000 means 1/1000 (0 means ignore) (uint16_t)
-                aperture                  : F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) (uint8_t)
-                iso                       : ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) (uint8_t)
-                exposure_type             : Exposure type enumeration from 1 to N (0 means ignore) (uint8_t)
-                command_id                : Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once (uint8_t)
-                engine_cut_off            : Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) (uint8_t)
-                extra_param               : Extra parameters enumeration (0 means ignore) (uint8_t)
-                extra_value               : Correspondent value to given extra_param (float)
-
-                '''
-                return self.send(self.digicam_configure_encode(target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value))
-            
-        def digicam_control_encode(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value):
-                '''
-                Control on-board Camera Control System to take shots.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                session                   : 0: stop, 1: start or keep it up //Session control e.g. show/hide lens (uint8_t)
-                zoom_pos                  : 1 to N //Zoom's absolute position (0 means ignore) (uint8_t)
-                zoom_step                 : -100 to 100 //Zooming step value to offset zoom from the current position (int8_t)
-                focus_lock                : 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus (uint8_t)
-                shot                      : 0: ignore, 1: shot or start filming (uint8_t)
-                command_id                : Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once (uint8_t)
-                extra_param               : Extra parameters enumeration (0 means ignore) (uint8_t)
-                extra_value               : Correspondent value to given extra_param (float)
-
-                '''
-                msg = MAVLink_digicam_control_message(target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value)
-                msg.pack(self)
-                return msg
-            
-        def digicam_control_send(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value):
-                '''
-                Control on-board Camera Control System to take shots.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                session                   : 0: stop, 1: start or keep it up //Session control e.g. show/hide lens (uint8_t)
-                zoom_pos                  : 1 to N //Zoom's absolute position (0 means ignore) (uint8_t)
-                zoom_step                 : -100 to 100 //Zooming step value to offset zoom from the current position (int8_t)
-                focus_lock                : 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus (uint8_t)
-                shot                      : 0: ignore, 1: shot or start filming (uint8_t)
-                command_id                : Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once (uint8_t)
-                extra_param               : Extra parameters enumeration (0 means ignore) (uint8_t)
-                extra_value               : Correspondent value to given extra_param (float)
-
-                '''
-                return self.send(self.digicam_control_encode(target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value))
-            
-        def mount_configure_encode(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw):
-                '''
-                Message to configure a camera mount, directional antenna, etc.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mount_mode                : mount operating mode (see MAV_MOUNT_MODE enum) (uint8_t)
-                stab_roll                 : (1 = yes, 0 = no) (uint8_t)
-                stab_pitch                : (1 = yes, 0 = no) (uint8_t)
-                stab_yaw                  : (1 = yes, 0 = no) (uint8_t)
-
-                '''
-                msg = MAVLink_mount_configure_message(target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw)
-                msg.pack(self)
-                return msg
-            
-        def mount_configure_send(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw):
-                '''
-                Message to configure a camera mount, directional antenna, etc.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mount_mode                : mount operating mode (see MAV_MOUNT_MODE enum) (uint8_t)
-                stab_roll                 : (1 = yes, 0 = no) (uint8_t)
-                stab_pitch                : (1 = yes, 0 = no) (uint8_t)
-                stab_yaw                  : (1 = yes, 0 = no) (uint8_t)
-
-                '''
-                return self.send(self.mount_configure_encode(target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw))
-            
-        def mount_control_encode(self, target_system, target_component, input_a, input_b, input_c, save_position):
-                '''
-                Message to control a camera mount, directional antenna, etc.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                input_a                   : pitch(deg*100) or lat, depending on mount mode (int32_t)
-                input_b                   : roll(deg*100) or lon depending on mount mode (int32_t)
-                input_c                   : yaw(deg*100) or alt (in cm) depending on mount mode (int32_t)
-                save_position             : if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) (uint8_t)
-
-                '''
-                msg = MAVLink_mount_control_message(target_system, target_component, input_a, input_b, input_c, save_position)
-                msg.pack(self)
-                return msg
-            
-        def mount_control_send(self, target_system, target_component, input_a, input_b, input_c, save_position):
-                '''
-                Message to control a camera mount, directional antenna, etc.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                input_a                   : pitch(deg*100) or lat, depending on mount mode (int32_t)
-                input_b                   : roll(deg*100) or lon depending on mount mode (int32_t)
-                input_c                   : yaw(deg*100) or alt (in cm) depending on mount mode (int32_t)
-                save_position             : if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) (uint8_t)
-
-                '''
-                return self.send(self.mount_control_encode(target_system, target_component, input_a, input_b, input_c, save_position))
-            
-        def mount_status_encode(self, target_system, target_component, pointing_a, pointing_b, pointing_c):
-                '''
-                Message with some status from APM to GCS about camera or antenna mount
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                pointing_a                : pitch(deg*100) or lat, depending on mount mode (int32_t)
-                pointing_b                : roll(deg*100) or lon depending on mount mode (int32_t)
-                pointing_c                : yaw(deg*100) or alt (in cm) depending on mount mode (int32_t)
-
-                '''
-                msg = MAVLink_mount_status_message(target_system, target_component, pointing_a, pointing_b, pointing_c)
-                msg.pack(self)
-                return msg
-            
-        def mount_status_send(self, target_system, target_component, pointing_a, pointing_b, pointing_c):
-                '''
-                Message with some status from APM to GCS about camera or antenna mount
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                pointing_a                : pitch(deg*100) or lat, depending on mount mode (int32_t)
-                pointing_b                : roll(deg*100) or lon depending on mount mode (int32_t)
-                pointing_c                : yaw(deg*100) or alt (in cm) depending on mount mode (int32_t)
-
-                '''
-                return self.send(self.mount_status_encode(target_system, target_component, pointing_a, pointing_b, pointing_c))
-            
-        def fence_point_encode(self, target_system, target_component, idx, count, lat, lng):
-                '''
-                A fence point. Used to set a point when from               GCS -> MAV.
-                Also used to return a point from MAV -> GCS
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
-                count                     : total number of points (for sanity checking) (uint8_t)
-                lat                       : Latitude of point (float)
-                lng                       : Longitude of point (float)
-
-                '''
-                msg = MAVLink_fence_point_message(target_system, target_component, idx, count, lat, lng)
-                msg.pack(self)
-                return msg
-            
-        def fence_point_send(self, target_system, target_component, idx, count, lat, lng):
-                '''
-                A fence point. Used to set a point when from               GCS -> MAV.
-                Also used to return a point from MAV -> GCS
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
-                count                     : total number of points (for sanity checking) (uint8_t)
-                lat                       : Latitude of point (float)
-                lng                       : Longitude of point (float)
-
-                '''
-                return self.send(self.fence_point_encode(target_system, target_component, idx, count, lat, lng))
-            
-        def fence_fetch_point_encode(self, target_system, target_component, idx):
-                '''
-                Request a current fence point from MAV
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
-
-                '''
-                msg = MAVLink_fence_fetch_point_message(target_system, target_component, idx)
-                msg.pack(self)
-                return msg
-            
-        def fence_fetch_point_send(self, target_system, target_component, idx):
-                '''
-                Request a current fence point from MAV
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
-
-                '''
-                return self.send(self.fence_fetch_point_encode(target_system, target_component, idx))
-            
-        def fence_status_encode(self, breach_status, breach_count, breach_type, breach_time):
-                '''
-                Status of geo-fencing. Sent in extended             status stream when
-                fencing enabled
-
-                breach_status             : 0 if currently inside fence, 1 if outside (uint8_t)
-                breach_count              : number of fence breaches (uint16_t)
-                breach_type               : last breach type (see FENCE_BREACH_* enum) (uint8_t)
-                breach_time               : time of last breach in milliseconds since boot (uint32_t)
-
-                '''
-                msg = MAVLink_fence_status_message(breach_status, breach_count, breach_type, breach_time)
-                msg.pack(self)
-                return msg
-            
-        def fence_status_send(self, breach_status, breach_count, breach_type, breach_time):
-                '''
-                Status of geo-fencing. Sent in extended             status stream when
-                fencing enabled
-
-                breach_status             : 0 if currently inside fence, 1 if outside (uint8_t)
-                breach_count              : number of fence breaches (uint16_t)
-                breach_type               : last breach type (see FENCE_BREACH_* enum) (uint8_t)
-                breach_time               : time of last breach in milliseconds since boot (uint32_t)
-
-                '''
-                return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time))
-            
-        def ahrs_encode(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw):
-                '''
-                Status of DCM attitude estimator
-
-                omegaIx                   : X gyro drift estimate rad/s (float)
-                omegaIy                   : Y gyro drift estimate rad/s (float)
-                omegaIz                   : Z gyro drift estimate rad/s (float)
-                accel_weight              : average accel_weight (float)
-                renorm_val                : average renormalisation value (float)
-                error_rp                  : average error_roll_pitch value (float)
-                error_yaw                 : average error_yaw value (float)
-
-                '''
-                msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)
-                msg.pack(self)
-                return msg
-            
-        def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw):
-                '''
-                Status of DCM attitude estimator
-
-                omegaIx                   : X gyro drift estimate rad/s (float)
-                omegaIy                   : Y gyro drift estimate rad/s (float)
-                omegaIz                   : Z gyro drift estimate rad/s (float)
-                accel_weight              : average accel_weight (float)
-                renorm_val                : average renormalisation value (float)
-                error_rp                  : average error_roll_pitch value (float)
-                error_yaw                 : average error_yaw value (float)
-
-                '''
-                return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw))
-            
-        def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro):
-                '''
-                Status of simulation environment, if used
-
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                xacc                      : X acceleration m/s/s (float)
-                yacc                      : Y acceleration m/s/s (float)
-                zacc                      : Z acceleration m/s/s (float)
-                xgyro                     : Angular speed around X axis rad/s (float)
-                ygyro                     : Angular speed around Y axis rad/s (float)
-                zgyro                     : Angular speed around Z axis rad/s (float)
-
-                '''
-                msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)
-                msg.pack(self)
-                return msg
-            
-        def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro):
-                '''
-                Status of simulation environment, if used
-
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                xacc                      : X acceleration m/s/s (float)
-                yacc                      : Y acceleration m/s/s (float)
-                zacc                      : Z acceleration m/s/s (float)
-                xgyro                     : Angular speed around X axis rad/s (float)
-                ygyro                     : Angular speed around Y axis rad/s (float)
-                zgyro                     : Angular speed around Z axis rad/s (float)
-
-                '''
-                return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro))
-            
-        def hwstatus_encode(self, Vcc, I2Cerr):
-                '''
-                Status of key hardware
-
-                Vcc                       : board voltage (mV) (uint16_t)
-                I2Cerr                    : I2C error count (uint8_t)
-
-                '''
-                msg = MAVLink_hwstatus_message(Vcc, I2Cerr)
-                msg.pack(self)
-                return msg
-            
-        def hwstatus_send(self, Vcc, I2Cerr):
-                '''
-                Status of key hardware
-
-                Vcc                       : board voltage (mV) (uint16_t)
-                I2Cerr                    : I2C error count (uint8_t)
-
-                '''
-                return self.send(self.hwstatus_encode(Vcc, I2Cerr))
-            
-        def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed):
-                '''
-                Status generated by radio
-
-                rssi                      : local signal strength (uint8_t)
-                remrssi                   : remote signal strength (uint8_t)
-                txbuf                     : how full the tx buffer is as a percentage (uint8_t)
-                noise                     : background noise level (uint8_t)
-                remnoise                  : remote background noise level (uint8_t)
-                rxerrors                  : receive errors (uint16_t)
-                fixed                     : count of error corrected packets (uint16_t)
-
-                '''
-                msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)
-                msg.pack(self)
-                return msg
-            
-        def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed):
-                '''
-                Status generated by radio
-
-                rssi                      : local signal strength (uint8_t)
-                remrssi                   : remote signal strength (uint8_t)
-                txbuf                     : how full the tx buffer is as a percentage (uint8_t)
-                noise                     : background noise level (uint8_t)
-                remnoise                  : remote background noise level (uint8_t)
-                rxerrors                  : receive errors (uint16_t)
-                fixed                     : count of error corrected packets (uint16_t)
-
-                '''
-                return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed))
-            
-        def heartbeat_encode(self, type, autopilot, mavlink_version=2):
-                '''
-                The heartbeat message shows that a system is present and responding.
-                The type of the MAV and Autopilot hardware allow the
-                receiving system to treat further messages from this
-                system appropriate (e.g. by laying out the user
-                interface based on the autopilot).
-
-                type                      : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t)
-                autopilot                 : Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM (uint8_t)
-                mavlink_version           : MAVLink version (uint8_t)
-
-                '''
-                msg = MAVLink_heartbeat_message(type, autopilot, mavlink_version)
-                msg.pack(self)
-                return msg
-            
-        def heartbeat_send(self, type, autopilot, mavlink_version=2):
-                '''
-                The heartbeat message shows that a system is present and responding.
-                The type of the MAV and Autopilot hardware allow the
-                receiving system to treat further messages from this
-                system appropriate (e.g. by laying out the user
-                interface based on the autopilot).
-
-                type                      : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t)
-                autopilot                 : Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM (uint8_t)
-                mavlink_version           : MAVLink version (uint8_t)
-
-                '''
-                return self.send(self.heartbeat_encode(type, autopilot, mavlink_version))
-            
-        def boot_encode(self, version):
-                '''
-                The boot message indicates that a system is starting. The onboard
-                software version allows to keep track of onboard
-                soft/firmware revisions.
-
-                version                   : The onboard software version (uint32_t)
-
-                '''
-                msg = MAVLink_boot_message(version)
-                msg.pack(self)
-                return msg
-            
-        def boot_send(self, version):
-                '''
-                The boot message indicates that a system is starting. The onboard
-                software version allows to keep track of onboard
-                soft/firmware revisions.
-
-                version                   : The onboard software version (uint32_t)
-
-                '''
-                return self.send(self.boot_encode(version))
-            
-        def system_time_encode(self, time_usec):
-                '''
-                The system time is the time of the master clock, typically the
-                computer clock of the main onboard computer.
-
-                time_usec                 : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t)
-
-                '''
-                msg = MAVLink_system_time_message(time_usec)
-                msg.pack(self)
-                return msg
-            
-        def system_time_send(self, time_usec):
-                '''
-                The system time is the time of the master clock, typically the
-                computer clock of the main onboard computer.
-
-                time_usec                 : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t)
-
-                '''
-                return self.send(self.system_time_encode(time_usec))
-            
-        def ping_encode(self, seq, target_system, target_component, time):
-                '''
-                A ping message either requesting or responding to a ping. This allows
-                to measure the system latencies, including serial
-                port, radio modem and UDP connections.
-
-                seq                       : PING sequence (uint32_t)
-                target_system             : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t)
-                target_component          : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t)
-                time                      : Unix timestamp in microseconds (uint64_t)
-
-                '''
-                msg = MAVLink_ping_message(seq, target_system, target_component, time)
-                msg.pack(self)
-                return msg
-            
-        def ping_send(self, seq, target_system, target_component, time):
-                '''
-                A ping message either requesting or responding to a ping. This allows
-                to measure the system latencies, including serial
-                port, radio modem and UDP connections.
-
-                seq                       : PING sequence (uint32_t)
-                target_system             : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t)
-                target_component          : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t)
-                time                      : Unix timestamp in microseconds (uint64_t)
-
-                '''
-                return self.send(self.ping_encode(seq, target_system, target_component, time))
-            
-        def system_time_utc_encode(self, utc_date, utc_time):
-                '''
-                UTC date and time from GPS module
-
-                utc_date                  : GPS UTC date ddmmyy (uint32_t)
-                utc_time                  : GPS UTC time hhmmss (uint32_t)
-
-                '''
-                msg = MAVLink_system_time_utc_message(utc_date, utc_time)
-                msg.pack(self)
-                return msg
-            
-        def system_time_utc_send(self, utc_date, utc_time):
-                '''
-                UTC date and time from GPS module
-
-                utc_date                  : GPS UTC date ddmmyy (uint32_t)
-                utc_time                  : GPS UTC time hhmmss (uint32_t)
-
-                '''
-                return self.send(self.system_time_utc_encode(utc_date, utc_time))
-            
-        def change_operator_control_encode(self, target_system, control_request, version, passkey):
-                '''
-                Request to control this MAV
-
-                target_system             : System the GCS requests control for (uint8_t)
-                control_request           : 0: request control of this MAV, 1: Release control of this MAV (uint8_t)
-                version                   : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t)
-                passkey                   : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char)
-
-                '''
-                msg = MAVLink_change_operator_control_message(target_system, control_request, version, passkey)
-                msg.pack(self)
-                return msg
-            
-        def change_operator_control_send(self, target_system, control_request, version, passkey):
-                '''
-                Request to control this MAV
-
-                target_system             : System the GCS requests control for (uint8_t)
-                control_request           : 0: request control of this MAV, 1: Release control of this MAV (uint8_t)
-                version                   : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t)
-                passkey                   : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char)
-
-                '''
-                return self.send(self.change_operator_control_encode(target_system, control_request, version, passkey))
-            
-        def change_operator_control_ack_encode(self, gcs_system_id, control_request, ack):
-                '''
-                Accept / deny control of this MAV
-
-                gcs_system_id             : ID of the GCS this message (uint8_t)
-                control_request           : 0: request control of this MAV, 1: Release control of this MAV (uint8_t)
-                ack                       : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t)
-
-                '''
-                msg = MAVLink_change_operator_control_ack_message(gcs_system_id, control_request, ack)
-                msg.pack(self)
-                return msg
-            
-        def change_operator_control_ack_send(self, gcs_system_id, control_request, ack):
-                '''
-                Accept / deny control of this MAV
-
-                gcs_system_id             : ID of the GCS this message (uint8_t)
-                control_request           : 0: request control of this MAV, 1: Release control of this MAV (uint8_t)
-                ack                       : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t)
-
-                '''
-                return self.send(self.change_operator_control_ack_encode(gcs_system_id, control_request, ack))
-            
-        def auth_key_encode(self, key):
-                '''
-                Emit an encrypted signature / key identifying this system. PLEASE
-                NOTE: This protocol has been kept simple, so
-                transmitting the key requires an encrypted channel for
-                true safety.
-
-                key                       : key (char)
-
-                '''
-                msg = MAVLink_auth_key_message(key)
-                msg.pack(self)
-                return msg
-            
-        def auth_key_send(self, key):
-                '''
-                Emit an encrypted signature / key identifying this system. PLEASE
-                NOTE: This protocol has been kept simple, so
-                transmitting the key requires an encrypted channel for
-                true safety.
-
-                key                       : key (char)
-
-                '''
-                return self.send(self.auth_key_encode(key))
-            
-        def action_ack_encode(self, action, result):
-                '''
-                This message acknowledges an action. IMPORTANT: The acknowledgement
-                can be also negative, e.g. the MAV rejects a reset
-                message because it is in-flight. The action ids are
-                defined in ENUM MAV_ACTION in
-                mavlink/include/mavlink_types.h
-
-                action                    : The action id (uint8_t)
-                result                    : 0: Action DENIED, 1: Action executed (uint8_t)
-
-                '''
-                msg = MAVLink_action_ack_message(action, result)
-                msg.pack(self)
-                return msg
-            
-        def action_ack_send(self, action, result):
-                '''
-                This message acknowledges an action. IMPORTANT: The acknowledgement
-                can be also negative, e.g. the MAV rejects a reset
-                message because it is in-flight. The action ids are
-                defined in ENUM MAV_ACTION in
-                mavlink/include/mavlink_types.h
-
-                action                    : The action id (uint8_t)
-                result                    : 0: Action DENIED, 1: Action executed (uint8_t)
-
-                '''
-                return self.send(self.action_ack_encode(action, result))
-            
-        def action_encode(self, target, target_component, action):
-                '''
-                An action message allows to execute a certain onboard action. These
-                include liftoff, land, storing parameters too EEPROM,
-                shutddown, etc. The action ids are defined in ENUM
-                MAV_ACTION in mavlink/include/mavlink_types.h
-
-                target                    : The system executing the action (uint8_t)
-                target_component          : The component executing the action (uint8_t)
-                action                    : The action id (uint8_t)
-
-                '''
-                msg = MAVLink_action_message(target, target_component, action)
-                msg.pack(self)
-                return msg
-            
-        def action_send(self, target, target_component, action):
-                '''
-                An action message allows to execute a certain onboard action. These
-                include liftoff, land, storing parameters too EEPROM,
-                shutddown, etc. The action ids are defined in ENUM
-                MAV_ACTION in mavlink/include/mavlink_types.h
-
-                target                    : The system executing the action (uint8_t)
-                target_component          : The component executing the action (uint8_t)
-                action                    : The action id (uint8_t)
-
-                '''
-                return self.send(self.action_encode(target, target_component, action))
-            
-        def set_mode_encode(self, target, mode):
-                '''
-                Set the system mode, as defined by enum MAV_MODE in
-                mavlink/include/mavlink_types.h. There is no target
-                component id as the mode is by definition for the
-                overall aircraft, not only for one component.
-
-                target                    : The system setting the mode (uint8_t)
-                mode                      : The new mode (uint8_t)
-
-                '''
-                msg = MAVLink_set_mode_message(target, mode)
-                msg.pack(self)
-                return msg
-            
-        def set_mode_send(self, target, mode):
-                '''
-                Set the system mode, as defined by enum MAV_MODE in
-                mavlink/include/mavlink_types.h. There is no target
-                component id as the mode is by definition for the
-                overall aircraft, not only for one component.
-
-                target                    : The system setting the mode (uint8_t)
-                mode                      : The new mode (uint8_t)
-
-                '''
-                return self.send(self.set_mode_encode(target, mode))
-            
-        def set_nav_mode_encode(self, target, nav_mode):
-                '''
-                Set the system navigation mode, as defined by enum MAV_NAV_MODE in
-                mavlink/include/mavlink_types.h. The navigation mode
-                applies to the whole aircraft and thus all components.
-
-                target                    : The system setting the mode (uint8_t)
-                nav_mode                  : The new navigation mode (uint8_t)
-
-                '''
-                msg = MAVLink_set_nav_mode_message(target, nav_mode)
-                msg.pack(self)
-                return msg
-            
-        def set_nav_mode_send(self, target, nav_mode):
-                '''
-                Set the system navigation mode, as defined by enum MAV_NAV_MODE in
-                mavlink/include/mavlink_types.h. The navigation mode
-                applies to the whole aircraft and thus all components.
-
-                target                    : The system setting the mode (uint8_t)
-                nav_mode                  : The new navigation mode (uint8_t)
-
-                '''
-                return self.send(self.set_nav_mode_encode(target, nav_mode))
-            
-        def param_request_read_encode(self, target_system, target_component, param_id, param_index):
-                '''
-                Request to read the onboard parameter with the param_id string id.
-                Onboard parameters are stored as key[const char*] ->
-                value[float]. This allows to send a parameter to any
-                other component (such as the GCS) without the need of
-                previous knowledge of possible parameter names. Thus
-                the same GCS can store different parameters for
-                different autopilots. See also
-                http://qgroundcontrol.org/parameter_interface for a
-                full documentation of QGroundControl and IMU code.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                param_id                  : Onboard parameter id (int8_t)
-                param_index               : Parameter index. Send -1 to use the param ID field as identifier (int16_t)
-
-                '''
-                msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index)
-                msg.pack(self)
-                return msg
-            
-        def param_request_read_send(self, target_system, target_component, param_id, param_index):
-                '''
-                Request to read the onboard parameter with the param_id string id.
-                Onboard parameters are stored as key[const char*] ->
-                value[float]. This allows to send a parameter to any
-                other component (such as the GCS) without the need of
-                previous knowledge of possible parameter names. Thus
-                the same GCS can store different parameters for
-                different autopilots. See also
-                http://qgroundcontrol.org/parameter_interface for a
-                full documentation of QGroundControl and IMU code.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                param_id                  : Onboard parameter id (int8_t)
-                param_index               : Parameter index. Send -1 to use the param ID field as identifier (int16_t)
-
-                '''
-                return self.send(self.param_request_read_encode(target_system, target_component, param_id, param_index))
-            
-        def param_request_list_encode(self, target_system, target_component):
-                '''
-                Request all parameters of this component. After his request, all
-                parameters are emitted.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                msg = MAVLink_param_request_list_message(target_system, target_component)
-                msg.pack(self)
-                return msg
-            
-        def param_request_list_send(self, target_system, target_component):
-                '''
-                Request all parameters of this component. After his request, all
-                parameters are emitted.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                return self.send(self.param_request_list_encode(target_system, target_component))
-            
-        def param_value_encode(self, param_id, param_value, param_count, param_index):
-                '''
-                Emit the value of a onboard parameter. The inclusion of param_count
-                and param_index in the message allows the recipient to
-                keep track of received parameters and allows him to
-                re-request missing parameters after a loss or timeout.
-
-                param_id                  : Onboard parameter id (int8_t)
-                param_value               : Onboard parameter value (float)
-                param_count               : Total number of onboard parameters (uint16_t)
-                param_index               : Index of this onboard parameter (uint16_t)
-
-                '''
-                msg = MAVLink_param_value_message(param_id, param_value, param_count, param_index)
-                msg.pack(self)
-                return msg
-            
-        def param_value_send(self, param_id, param_value, param_count, param_index):
-                '''
-                Emit the value of a onboard parameter. The inclusion of param_count
-                and param_index in the message allows the recipient to
-                keep track of received parameters and allows him to
-                re-request missing parameters after a loss or timeout.
-
-                param_id                  : Onboard parameter id (int8_t)
-                param_value               : Onboard parameter value (float)
-                param_count               : Total number of onboard parameters (uint16_t)
-                param_index               : Index of this onboard parameter (uint16_t)
-
-                '''
-                return self.send(self.param_value_encode(param_id, param_value, param_count, param_index))
-            
-        def param_set_encode(self, target_system, target_component, param_id, param_value):
-                '''
-                Set a parameter value TEMPORARILY to RAM. It will be reset to default
-                on system reboot. Send the ACTION
-                MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM
-                contents to EEPROM. IMPORTANT: The receiving component
-                should acknowledge the new parameter value by sending
-                a param_value message to all communication partners.
-                This will also ensure that multiple GCS all have an
-                up-to-date list of all parameters. If the sending GCS
-                did not receive a PARAM_VALUE message within its
-                timeout time, it should re-send the PARAM_SET message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                param_id                  : Onboard parameter id (int8_t)
-                param_value               : Onboard parameter value (float)
-
-                '''
-                msg = MAVLink_param_set_message(target_system, target_component, param_id, param_value)
-                msg.pack(self)
-                return msg
-            
-        def param_set_send(self, target_system, target_component, param_id, param_value):
-                '''
-                Set a parameter value TEMPORARILY to RAM. It will be reset to default
-                on system reboot. Send the ACTION
-                MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM
-                contents to EEPROM. IMPORTANT: The receiving component
-                should acknowledge the new parameter value by sending
-                a param_value message to all communication partners.
-                This will also ensure that multiple GCS all have an
-                up-to-date list of all parameters. If the sending GCS
-                did not receive a PARAM_VALUE message within its
-                timeout time, it should re-send the PARAM_SET message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                param_id                  : Onboard parameter id (int8_t)
-                param_value               : Onboard parameter value (float)
-
-                '''
-                return self.send(self.param_set_encode(target_system, target_component, param_id, param_value))
-            
-        def gps_raw_int_encode(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg):
-                '''
-                The global position, as returned by the Global Positioning System
-                (GPS). This is NOT the global position estimate of the
-                sytem, but rather a RAW sensor value. See message
-                GLOBAL_POSITION for the global position estimate.
-                Coordinate frame is right-handed, Z-axis up (GPS
-                frame)
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_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. (uint8_t)
-                lat                       : Latitude in 1E7 degrees (int32_t)
-                lon                       : Longitude in 1E7 degrees (int32_t)
-                alt                       : Altitude in 1E3 meters (millimeters) (int32_t)
-                eph                       : GPS HDOP (float)
-                epv                       : GPS VDOP (float)
-                v                         : GPS ground speed (m/s) (float)
-                hdg                       : Compass heading in degrees, 0..360 degrees (float)
-
-                '''
-                msg = MAVLink_gps_raw_int_message(usec, fix_type, lat, lon, alt, eph, epv, v, hdg)
-                msg.pack(self)
-                return msg
-            
-        def gps_raw_int_send(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg):
-                '''
-                The global position, as returned by the Global Positioning System
-                (GPS). This is NOT the global position estimate of the
-                sytem, but rather a RAW sensor value. See message
-                GLOBAL_POSITION for the global position estimate.
-                Coordinate frame is right-handed, Z-axis up (GPS
-                frame)
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_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. (uint8_t)
-                lat                       : Latitude in 1E7 degrees (int32_t)
-                lon                       : Longitude in 1E7 degrees (int32_t)
-                alt                       : Altitude in 1E3 meters (millimeters) (int32_t)
-                eph                       : GPS HDOP (float)
-                epv                       : GPS VDOP (float)
-                v                         : GPS ground speed (m/s) (float)
-                hdg                       : Compass heading in degrees, 0..360 degrees (float)
-
-                '''
-                return self.send(self.gps_raw_int_encode(usec, fix_type, lat, lon, alt, eph, epv, v, hdg))
-            
-        def scaled_imu_encode(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                '''
-                The RAW IMU readings for the usual 9DOF sensor setup. This message
-                should contain the scaled values to the described
-                units
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                xacc                      : X acceleration (mg) (int16_t)
-                yacc                      : Y acceleration (mg) (int16_t)
-                zacc                      : Z acceleration (mg) (int16_t)
-                xgyro                     : Angular speed around X axis (millirad /sec) (int16_t)
-                ygyro                     : Angular speed around Y axis (millirad /sec) (int16_t)
-                zgyro                     : Angular speed around Z axis (millirad /sec) (int16_t)
-                xmag                      : X Magnetic field (milli tesla) (int16_t)
-                ymag                      : Y Magnetic field (milli tesla) (int16_t)
-                zmag                      : Z Magnetic field (milli tesla) (int16_t)
-
-                '''
-                msg = MAVLink_scaled_imu_message(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)
-                msg.pack(self)
-                return msg
-            
-        def scaled_imu_send(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                '''
-                The RAW IMU readings for the usual 9DOF sensor setup. This message
-                should contain the scaled values to the described
-                units
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                xacc                      : X acceleration (mg) (int16_t)
-                yacc                      : Y acceleration (mg) (int16_t)
-                zacc                      : Z acceleration (mg) (int16_t)
-                xgyro                     : Angular speed around X axis (millirad /sec) (int16_t)
-                ygyro                     : Angular speed around Y axis (millirad /sec) (int16_t)
-                zgyro                     : Angular speed around Z axis (millirad /sec) (int16_t)
-                xmag                      : X Magnetic field (milli tesla) (int16_t)
-                ymag                      : Y Magnetic field (milli tesla) (int16_t)
-                zmag                      : Z Magnetic field (milli tesla) (int16_t)
-
-                '''
-                return self.send(self.scaled_imu_encode(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag))
-            
-        def gps_status_encode(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr):
-                '''
-                The positioning status, as reported by GPS. This message is intended
-                to display status information about each satellite
-                visible to the receiver. See message GLOBAL_POSITION
-                for the global position estimate. This message can
-                contain information for up to 20 satellites.
-
-                satellites_visible        : Number of satellites visible (uint8_t)
-                satellite_prn             : Global satellite ID (int8_t)
-                satellite_used            : 0: Satellite not used, 1: used for localization (int8_t)
-                satellite_elevation        : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (int8_t)
-                satellite_azimuth         : Direction of satellite, 0: 0 deg, 255: 360 deg. (int8_t)
-                satellite_snr             : Signal to noise ratio of satellite (int8_t)
-
-                '''
-                msg = MAVLink_gps_status_message(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr)
-                msg.pack(self)
-                return msg
-            
-        def gps_status_send(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr):
-                '''
-                The positioning status, as reported by GPS. This message is intended
-                to display status information about each satellite
-                visible to the receiver. See message GLOBAL_POSITION
-                for the global position estimate. This message can
-                contain information for up to 20 satellites.
-
-                satellites_visible        : Number of satellites visible (uint8_t)
-                satellite_prn             : Global satellite ID (int8_t)
-                satellite_used            : 0: Satellite not used, 1: used for localization (int8_t)
-                satellite_elevation        : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (int8_t)
-                satellite_azimuth         : Direction of satellite, 0: 0 deg, 255: 360 deg. (int8_t)
-                satellite_snr             : Signal to noise ratio of satellite (int8_t)
-
-                '''
-                return self.send(self.gps_status_encode(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr))
-            
-        def raw_imu_encode(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                '''
-                The RAW IMU readings for the usual 9DOF sensor setup. This message
-                should always contain the true raw values without any
-                scaling to allow data capture and system debugging.
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                xacc                      : X acceleration (raw) (int16_t)
-                yacc                      : Y acceleration (raw) (int16_t)
-                zacc                      : Z acceleration (raw) (int16_t)
-                xgyro                     : Angular speed around X axis (raw) (int16_t)
-                ygyro                     : Angular speed around Y axis (raw) (int16_t)
-                zgyro                     : Angular speed around Z axis (raw) (int16_t)
-                xmag                      : X Magnetic field (raw) (int16_t)
-                ymag                      : Y Magnetic field (raw) (int16_t)
-                zmag                      : Z Magnetic field (raw) (int16_t)
-
-                '''
-                msg = MAVLink_raw_imu_message(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)
-                msg.pack(self)
-                return msg
-            
-        def raw_imu_send(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                '''
-                The RAW IMU readings for the usual 9DOF sensor setup. This message
-                should always contain the true raw values without any
-                scaling to allow data capture and system debugging.
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                xacc                      : X acceleration (raw) (int16_t)
-                yacc                      : Y acceleration (raw) (int16_t)
-                zacc                      : Z acceleration (raw) (int16_t)
-                xgyro                     : Angular speed around X axis (raw) (int16_t)
-                ygyro                     : Angular speed around Y axis (raw) (int16_t)
-                zgyro                     : Angular speed around Z axis (raw) (int16_t)
-                xmag                      : X Magnetic field (raw) (int16_t)
-                ymag                      : Y Magnetic field (raw) (int16_t)
-                zmag                      : Z Magnetic field (raw) (int16_t)
-
-                '''
-                return self.send(self.raw_imu_encode(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag))
-            
-        def raw_pressure_encode(self, usec, press_abs, press_diff1, press_diff2, temperature):
-                '''
-                The RAW pressure readings for the typical setup of one absolute
-                pressure and one differential pressure sensor. The
-                sensor values should be the raw, UNSCALED ADC values.
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                press_abs                 : Absolute pressure (raw) (int16_t)
-                press_diff1               : Differential pressure 1 (raw) (int16_t)
-                press_diff2               : Differential pressure 2 (raw) (int16_t)
-                temperature               : Raw Temperature measurement (raw) (int16_t)
-
-                '''
-                msg = MAVLink_raw_pressure_message(usec, press_abs, press_diff1, press_diff2, temperature)
-                msg.pack(self)
-                return msg
-            
-        def raw_pressure_send(self, usec, press_abs, press_diff1, press_diff2, temperature):
-                '''
-                The RAW pressure readings for the typical setup of one absolute
-                pressure and one differential pressure sensor. The
-                sensor values should be the raw, UNSCALED ADC values.
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                press_abs                 : Absolute pressure (raw) (int16_t)
-                press_diff1               : Differential pressure 1 (raw) (int16_t)
-                press_diff2               : Differential pressure 2 (raw) (int16_t)
-                temperature               : Raw Temperature measurement (raw) (int16_t)
-
-                '''
-                return self.send(self.raw_pressure_encode(usec, press_abs, press_diff1, press_diff2, temperature))
-            
-        def scaled_pressure_encode(self, usec, press_abs, press_diff, temperature):
-                '''
-                The pressure readings for the typical setup of one absolute and
-                differential pressure sensor. The units are as
-                specified in each field.
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                press_abs                 : Absolute pressure (hectopascal) (float)
-                press_diff                : Differential pressure 1 (hectopascal) (float)
-                temperature               : Temperature measurement (0.01 degrees celsius) (int16_t)
-
-                '''
-                msg = MAVLink_scaled_pressure_message(usec, press_abs, press_diff, temperature)
-                msg.pack(self)
-                return msg
-            
-        def scaled_pressure_send(self, usec, press_abs, press_diff, temperature):
-                '''
-                The pressure readings for the typical setup of one absolute and
-                differential pressure sensor. The units are as
-                specified in each field.
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                press_abs                 : Absolute pressure (hectopascal) (float)
-                press_diff                : Differential pressure 1 (hectopascal) (float)
-                temperature               : Temperature measurement (0.01 degrees celsius) (int16_t)
-
-                '''
-                return self.send(self.scaled_pressure_encode(usec, press_abs, press_diff, temperature))
-            
-        def attitude_encode(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed):
-                '''
-                The attitude in the aeronautical frame (right-handed, Z-down, X-front,
-                Y-right).
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-
-                '''
-                msg = MAVLink_attitude_message(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed)
-                msg.pack(self)
-                return msg
-            
-        def attitude_send(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed):
-                '''
-                The attitude in the aeronautical frame (right-handed, Z-down, X-front,
-                Y-right).
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-
-                '''
-                return self.send(self.attitude_encode(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed))
-            
-        def local_position_encode(self, usec, x, y, z, vx, vy, vz):
-                '''
-                The filtered local position (e.g. fused computer vision and
-                accelerometers). Coordinate frame is right-handed,
-                Z-axis down (aeronautical frame)
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                x                         : X Position (float)
-                y                         : Y Position (float)
-                z                         : Z Position (float)
-                vx                        : X Speed (float)
-                vy                        : Y Speed (float)
-                vz                        : Z Speed (float)
-
-                '''
-                msg = MAVLink_local_position_message(usec, x, y, z, vx, vy, vz)
-                msg.pack(self)
-                return msg
-            
-        def local_position_send(self, usec, x, y, z, vx, vy, vz):
-                '''
-                The filtered local position (e.g. fused computer vision and
-                accelerometers). Coordinate frame is right-handed,
-                Z-axis down (aeronautical frame)
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                x                         : X Position (float)
-                y                         : Y Position (float)
-                z                         : Z Position (float)
-                vx                        : X Speed (float)
-                vy                        : Y Speed (float)
-                vz                        : Z Speed (float)
-
-                '''
-                return self.send(self.local_position_encode(usec, x, y, z, vx, vy, vz))
-            
-        def global_position_encode(self, usec, lat, lon, alt, vx, vy, vz):
-                '''
-                The filtered global position (e.g. fused GPS and accelerometers).
-                Coordinate frame is right-handed, Z-axis up (GPS
-                frame)
-
-                usec                      : Timestamp (microseconds since unix epoch) (uint64_t)
-                lat                       : Latitude, in degrees (float)
-                lon                       : Longitude, in degrees (float)
-                alt                       : Absolute altitude, in meters (float)
-                vx                        : X Speed (in Latitude direction, positive: going north) (float)
-                vy                        : Y Speed (in Longitude direction, positive: going east) (float)
-                vz                        : Z Speed (in Altitude direction, positive: going up) (float)
-
-                '''
-                msg = MAVLink_global_position_message(usec, lat, lon, alt, vx, vy, vz)
-                msg.pack(self)
-                return msg
-            
-        def global_position_send(self, usec, lat, lon, alt, vx, vy, vz):
-                '''
-                The filtered global position (e.g. fused GPS and accelerometers).
-                Coordinate frame is right-handed, Z-axis up (GPS
-                frame)
-
-                usec                      : Timestamp (microseconds since unix epoch) (uint64_t)
-                lat                       : Latitude, in degrees (float)
-                lon                       : Longitude, in degrees (float)
-                alt                       : Absolute altitude, in meters (float)
-                vx                        : X Speed (in Latitude direction, positive: going north) (float)
-                vy                        : Y Speed (in Longitude direction, positive: going east) (float)
-                vz                        : Z Speed (in Altitude direction, positive: going up) (float)
-
-                '''
-                return self.send(self.global_position_encode(usec, lat, lon, alt, vx, vy, vz))
-            
-        def gps_raw_encode(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg):
-                '''
-                The global position, as returned by the Global Positioning System
-                (GPS). This is NOT the global position estimate of the
-                sytem, but rather a RAW sensor value. See message
-                GLOBAL_POSITION for the global position estimate.
-                Coordinate frame is right-handed, Z-axis up (GPS
-                frame)
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_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. (uint8_t)
-                lat                       : Latitude in degrees (float)
-                lon                       : Longitude in degrees (float)
-                alt                       : Altitude in meters (float)
-                eph                       : GPS HDOP (float)
-                epv                       : GPS VDOP (float)
-                v                         : GPS ground speed (float)
-                hdg                       : Compass heading in degrees, 0..360 degrees (float)
-
-                '''
-                msg = MAVLink_gps_raw_message(usec, fix_type, lat, lon, alt, eph, epv, v, hdg)
-                msg.pack(self)
-                return msg
-            
-        def gps_raw_send(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg):
-                '''
-                The global position, as returned by the Global Positioning System
-                (GPS). This is NOT the global position estimate of the
-                sytem, but rather a RAW sensor value. See message
-                GLOBAL_POSITION for the global position estimate.
-                Coordinate frame is right-handed, Z-axis up (GPS
-                frame)
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_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. (uint8_t)
-                lat                       : Latitude in degrees (float)
-                lon                       : Longitude in degrees (float)
-                alt                       : Altitude in meters (float)
-                eph                       : GPS HDOP (float)
-                epv                       : GPS VDOP (float)
-                v                         : GPS ground speed (float)
-                hdg                       : Compass heading in degrees, 0..360 degrees (float)
-
-                '''
-                return self.send(self.gps_raw_encode(usec, fix_type, lat, lon, alt, eph, epv, v, hdg))
-            
-        def sys_status_encode(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop):
-                '''
-                The general system state. If the system is following the MAVLink
-                standard, the system state is mainly defined by three
-                orthogonal states/modes: The system mode, which is
-                either LOCKED (motors shut down and locked), MANUAL
-                (system under RC control), GUIDED (system with
-                autonomous position control, position setpoint
-                controlled manually) or AUTO (system guided by
-                path/waypoint planner). The NAV_MODE defined the
-                current flight state: LIFTOFF (often an open-loop
-                maneuver), LANDING, WAYPOINTS or VECTOR. This
-                represents the internal navigation state machine. The
-                system status shows wether the system is currently
-                active or not and if an emergency occured. During the
-                CRITICAL and EMERGENCY states the MAV is still
-                considered to be active, but should start emergency
-                procedures autonomously. After a failure occured it
-                should first move from active to critical to allow
-                manual intervention and then move to emergency after a
-                certain timeout.
-
-                mode                      : System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h (uint8_t)
-                nav_mode                  : Navigation mode, see MAV_NAV_MODE ENUM (uint8_t)
-                status                    : System status flag, see MAV_STATUS ENUM (uint8_t)
-                load                      : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t)
-                vbat                      : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t)
-                battery_remaining         : Remaining battery energy: (0%: 0, 100%: 1000) (uint16_t)
-                packet_drop               : Dropped packets (packets that were corrupted on reception on the MAV) (uint16_t)
-
-                '''
-                msg = MAVLink_sys_status_message(mode, nav_mode, status, load, vbat, battery_remaining, packet_drop)
-                msg.pack(self)
-                return msg
-            
-        def sys_status_send(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop):
-                '''
-                The general system state. If the system is following the MAVLink
-                standard, the system state is mainly defined by three
-                orthogonal states/modes: The system mode, which is
-                either LOCKED (motors shut down and locked), MANUAL
-                (system under RC control), GUIDED (system with
-                autonomous position control, position setpoint
-                controlled manually) or AUTO (system guided by
-                path/waypoint planner). The NAV_MODE defined the
-                current flight state: LIFTOFF (often an open-loop
-                maneuver), LANDING, WAYPOINTS or VECTOR. This
-                represents the internal navigation state machine. The
-                system status shows wether the system is currently
-                active or not and if an emergency occured. During the
-                CRITICAL and EMERGENCY states the MAV is still
-                considered to be active, but should start emergency
-                procedures autonomously. After a failure occured it
-                should first move from active to critical to allow
-                manual intervention and then move to emergency after a
-                certain timeout.
-
-                mode                      : System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h (uint8_t)
-                nav_mode                  : Navigation mode, see MAV_NAV_MODE ENUM (uint8_t)
-                status                    : System status flag, see MAV_STATUS ENUM (uint8_t)
-                load                      : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t)
-                vbat                      : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t)
-                battery_remaining         : Remaining battery energy: (0%: 0, 100%: 1000) (uint16_t)
-                packet_drop               : Dropped packets (packets that were corrupted on reception on the MAV) (uint16_t)
-
-                '''
-                return self.send(self.sys_status_encode(mode, nav_mode, status, load, vbat, battery_remaining, packet_drop))
-            
-        def rc_channels_raw_encode(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi):
-                '''
-                The RAW values of the RC channels received. The standard PPM
-                modulation is as follows: 1000 microseconds: 0%, 2000
-                microseconds: 100%. Individual receivers/transmitters
-                might violate this specification.
-
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                msg = MAVLink_rc_channels_raw_message(chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi)
-                msg.pack(self)
-                return msg
-            
-        def rc_channels_raw_send(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi):
-                '''
-                The RAW values of the RC channels received. The standard PPM
-                modulation is as follows: 1000 microseconds: 0%, 2000
-                microseconds: 100%. Individual receivers/transmitters
-                might violate this specification.
-
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                return self.send(self.rc_channels_raw_encode(chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi))
-            
-        def rc_channels_scaled_encode(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi):
-                '''
-                The scaled values of the RC channels received. (-100%) -10000, (0%) 0,
-                (100%) 10000
-
-                chan1_scaled              : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan2_scaled              : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan3_scaled              : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan4_scaled              : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan5_scaled              : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan6_scaled              : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan7_scaled              : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan8_scaled              : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                msg = MAVLink_rc_channels_scaled_message(chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi)
-                msg.pack(self)
-                return msg
-            
-        def rc_channels_scaled_send(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi):
-                '''
-                The scaled values of the RC channels received. (-100%) -10000, (0%) 0,
-                (100%) 10000
-
-                chan1_scaled              : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan2_scaled              : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan3_scaled              : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan4_scaled              : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan5_scaled              : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan6_scaled              : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan7_scaled              : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan8_scaled              : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                return self.send(self.rc_channels_scaled_encode(chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi))
-            
-        def servo_output_raw_encode(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw):
-                '''
-                The RAW values of the servo outputs (for RC input from the remote, use
-                the RC_CHANNELS messages). The standard PPM modulation
-                is as follows: 1000 microseconds: 0%, 2000
-                microseconds: 100%.
-
-                servo1_raw                : Servo output 1 value, in microseconds (uint16_t)
-                servo2_raw                : Servo output 2 value, in microseconds (uint16_t)
-                servo3_raw                : Servo output 3 value, in microseconds (uint16_t)
-                servo4_raw                : Servo output 4 value, in microseconds (uint16_t)
-                servo5_raw                : Servo output 5 value, in microseconds (uint16_t)
-                servo6_raw                : Servo output 6 value, in microseconds (uint16_t)
-                servo7_raw                : Servo output 7 value, in microseconds (uint16_t)
-                servo8_raw                : Servo output 8 value, in microseconds (uint16_t)
-
-                '''
-                msg = MAVLink_servo_output_raw_message(servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw)
-                msg.pack(self)
-                return msg
-            
-        def servo_output_raw_send(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw):
-                '''
-                The RAW values of the servo outputs (for RC input from the remote, use
-                the RC_CHANNELS messages). The standard PPM modulation
-                is as follows: 1000 microseconds: 0%, 2000
-                microseconds: 100%.
-
-                servo1_raw                : Servo output 1 value, in microseconds (uint16_t)
-                servo2_raw                : Servo output 2 value, in microseconds (uint16_t)
-                servo3_raw                : Servo output 3 value, in microseconds (uint16_t)
-                servo4_raw                : Servo output 4 value, in microseconds (uint16_t)
-                servo5_raw                : Servo output 5 value, in microseconds (uint16_t)
-                servo6_raw                : Servo output 6 value, in microseconds (uint16_t)
-                servo7_raw                : Servo output 7 value, in microseconds (uint16_t)
-                servo8_raw                : Servo output 8 value, in microseconds (uint16_t)
-
-                '''
-                return self.send(self.servo_output_raw_encode(servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw))
-            
-        def waypoint_encode(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z):
-                '''
-                Message encoding a waypoint. This message is emitted to announce
-                the presence of a waypoint and to set a waypoint on
-                the system. The waypoint can be either in x, y, z
-                meters (type: LOCAL) or x:lat, y:lon, z:altitude.
-                Local frame is Z-down, right handed, global frame is
-                Z-up, right handed
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-                frame                     : The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h (uint8_t)
-                command                   : The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs (uint8_t)
-                current                   : false:0, true:1 (uint8_t)
-                autocontinue              : autocontinue to next wp (uint8_t)
-                param1                    : PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters (float)
-                param2                    : PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float)
-                param3                    : PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float)
-                param4                    : PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH (float)
-                x                         : PARAM5 / local: x position, global: latitude (float)
-                y                         : PARAM6 / y position: global: longitude (float)
-                z                         : PARAM7 / z position: global: altitude (float)
-
-                '''
-                msg = MAVLink_waypoint_message(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_send(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z):
-                '''
-                Message encoding a waypoint. This message is emitted to announce
-                the presence of a waypoint and to set a waypoint on
-                the system. The waypoint can be either in x, y, z
-                meters (type: LOCAL) or x:lat, y:lon, z:altitude.
-                Local frame is Z-down, right handed, global frame is
-                Z-up, right handed
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-                frame                     : The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h (uint8_t)
-                command                   : The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs (uint8_t)
-                current                   : false:0, true:1 (uint8_t)
-                autocontinue              : autocontinue to next wp (uint8_t)
-                param1                    : PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters (float)
-                param2                    : PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float)
-                param3                    : PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float)
-                param4                    : PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH (float)
-                x                         : PARAM5 / local: x position, global: latitude (float)
-                y                         : PARAM6 / y position: global: longitude (float)
-                z                         : PARAM7 / z position: global: altitude (float)
-
-                '''
-                return self.send(self.waypoint_encode(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z))
-            
-        def waypoint_request_encode(self, target_system, target_component, seq):
-                '''
-                Request the information of the waypoint with the sequence number seq.
-                The response of the system to this message should be a
-                WAYPOINT message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_waypoint_request_message(target_system, target_component, seq)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_request_send(self, target_system, target_component, seq):
-                '''
-                Request the information of the waypoint with the sequence number seq.
-                The response of the system to this message should be a
-                WAYPOINT message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-
-                '''
-                return self.send(self.waypoint_request_encode(target_system, target_component, seq))
-            
-        def waypoint_set_current_encode(self, target_system, target_component, seq):
-                '''
-                Set the waypoint with sequence number seq as current waypoint. This
-                means that the MAV will continue to this waypoint on
-                the shortest path (not following the waypoints in-
-                between).
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_waypoint_set_current_message(target_system, target_component, seq)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_set_current_send(self, target_system, target_component, seq):
-                '''
-                Set the waypoint with sequence number seq as current waypoint. This
-                means that the MAV will continue to this waypoint on
-                the shortest path (not following the waypoints in-
-                between).
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-
-                '''
-                return self.send(self.waypoint_set_current_encode(target_system, target_component, seq))
-            
-        def waypoint_current_encode(self, seq):
-                '''
-                Message that announces the sequence number of the current active
-                waypoint. The MAV will fly towards this waypoint.
-
-                seq                       : Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_waypoint_current_message(seq)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_current_send(self, seq):
-                '''
-                Message that announces the sequence number of the current active
-                waypoint. The MAV will fly towards this waypoint.
-
-                seq                       : Sequence (uint16_t)
-
-                '''
-                return self.send(self.waypoint_current_encode(seq))
-            
-        def waypoint_request_list_encode(self, target_system, target_component):
-                '''
-                Request the overall list of waypoints from the system/component.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                msg = MAVLink_waypoint_request_list_message(target_system, target_component)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_request_list_send(self, target_system, target_component):
-                '''
-                Request the overall list of waypoints from the system/component.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                return self.send(self.waypoint_request_list_encode(target_system, target_component))
-            
-        def waypoint_count_encode(self, target_system, target_component, count):
-                '''
-                This message is emitted as response to WAYPOINT_REQUEST_LIST by the
-                MAV. The GCS can then request the individual waypoints
-                based on the knowledge of the total number of
-                waypoints.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                count                     : Number of Waypoints in the Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_waypoint_count_message(target_system, target_component, count)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_count_send(self, target_system, target_component, count):
-                '''
-                This message is emitted as response to WAYPOINT_REQUEST_LIST by the
-                MAV. The GCS can then request the individual waypoints
-                based on the knowledge of the total number of
-                waypoints.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                count                     : Number of Waypoints in the Sequence (uint16_t)
-
-                '''
-                return self.send(self.waypoint_count_encode(target_system, target_component, count))
-            
-        def waypoint_clear_all_encode(self, target_system, target_component):
-                '''
-                Delete all waypoints at once.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                msg = MAVLink_waypoint_clear_all_message(target_system, target_component)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_clear_all_send(self, target_system, target_component):
-                '''
-                Delete all waypoints at once.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                return self.send(self.waypoint_clear_all_encode(target_system, target_component))
-            
-        def waypoint_reached_encode(self, seq):
-                '''
-                A certain waypoint has been reached. The system will either hold this
-                position (or circle on the orbit) or (if the
-                autocontinue on the WP was set) continue to the next
-                waypoint.
-
-                seq                       : Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_waypoint_reached_message(seq)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_reached_send(self, seq):
-                '''
-                A certain waypoint has been reached. The system will either hold this
-                position (or circle on the orbit) or (if the
-                autocontinue on the WP was set) continue to the next
-                waypoint.
-
-                seq                       : Sequence (uint16_t)
-
-                '''
-                return self.send(self.waypoint_reached_encode(seq))
-            
-        def waypoint_ack_encode(self, target_system, target_component, type):
-                '''
-                Ack message during waypoint handling. The type field states if this
-                message is a positive ack (type=0) or if an error
-                happened (type=non-zero).
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                type                      : 0: OK, 1: Error (uint8_t)
-
-                '''
-                msg = MAVLink_waypoint_ack_message(target_system, target_component, type)
-                msg.pack(self)
-                return msg
-            
-        def waypoint_ack_send(self, target_system, target_component, type):
-                '''
-                Ack message during waypoint handling. The type field states if this
-                message is a positive ack (type=0) or if an error
-                happened (type=non-zero).
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                type                      : 0: OK, 1: Error (uint8_t)
-
-                '''
-                return self.send(self.waypoint_ack_encode(target_system, target_component, type))
-            
-        def gps_set_global_origin_encode(self, target_system, target_component, latitude, longitude, altitude):
-                '''
-                As local waypoints exist, the global waypoint reference allows to
-                transform between the local coordinate frame and the
-                global (GPS) coordinate frame. This can be necessary
-                when e.g. in- and outdoor settings are connected and
-                the MAV should move from in- to outdoor.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                latitude                  : global position * 1E7 (int32_t)
-                longitude                 : global position * 1E7 (int32_t)
-                altitude                  : global position * 1000 (int32_t)
-
-                '''
-                msg = MAVLink_gps_set_global_origin_message(target_system, target_component, latitude, longitude, altitude)
-                msg.pack(self)
-                return msg
-            
-        def gps_set_global_origin_send(self, target_system, target_component, latitude, longitude, altitude):
-                '''
-                As local waypoints exist, the global waypoint reference allows to
-                transform between the local coordinate frame and the
-                global (GPS) coordinate frame. This can be necessary
-                when e.g. in- and outdoor settings are connected and
-                the MAV should move from in- to outdoor.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                latitude                  : global position * 1E7 (int32_t)
-                longitude                 : global position * 1E7 (int32_t)
-                altitude                  : global position * 1000 (int32_t)
-
-                '''
-                return self.send(self.gps_set_global_origin_encode(target_system, target_component, latitude, longitude, altitude))
-            
-        def gps_local_origin_set_encode(self, latitude, longitude, altitude):
-                '''
-                Once the MAV sets a new GPS-Local correspondence, this message
-                announces the origin (0,0,0) position
-
-                latitude                  : Latitude (WGS84), expressed as * 1E7 (int32_t)
-                longitude                 : Longitude (WGS84), expressed as * 1E7 (int32_t)
-                altitude                  : Altitude(WGS84), expressed as * 1000 (int32_t)
-
-                '''
-                msg = MAVLink_gps_local_origin_set_message(latitude, longitude, altitude)
-                msg.pack(self)
-                return msg
-            
-        def gps_local_origin_set_send(self, latitude, longitude, altitude):
-                '''
-                Once the MAV sets a new GPS-Local correspondence, this message
-                announces the origin (0,0,0) position
-
-                latitude                  : Latitude (WGS84), expressed as * 1E7 (int32_t)
-                longitude                 : Longitude (WGS84), expressed as * 1E7 (int32_t)
-                altitude                  : Altitude(WGS84), expressed as * 1000 (int32_t)
-
-                '''
-                return self.send(self.gps_local_origin_set_encode(latitude, longitude, altitude))
-            
-        def local_position_setpoint_set_encode(self, target_system, target_component, x, y, z, yaw):
-                '''
-                Set the setpoint for a local position controller. This is the position
-                in local coordinates the MAV should fly to. This
-                message is sent by the path/waypoint planner to the
-                onboard position controller. As some MAVs have a
-                degree of freedom in yaw (e.g. all
-                helicopters/quadrotors), the desired yaw angle is part
-                of the message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : Desired yaw angle (float)
-
-                '''
-                msg = MAVLink_local_position_setpoint_set_message(target_system, target_component, x, y, z, yaw)
-                msg.pack(self)
-                return msg
-            
-        def local_position_setpoint_set_send(self, target_system, target_component, x, y, z, yaw):
-                '''
-                Set the setpoint for a local position controller. This is the position
-                in local coordinates the MAV should fly to. This
-                message is sent by the path/waypoint planner to the
-                onboard position controller. As some MAVs have a
-                degree of freedom in yaw (e.g. all
-                helicopters/quadrotors), the desired yaw angle is part
-                of the message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : Desired yaw angle (float)
-
-                '''
-                return self.send(self.local_position_setpoint_set_encode(target_system, target_component, x, y, z, yaw))
-            
-        def local_position_setpoint_encode(self, x, y, z, yaw):
-                '''
-                Transmit the current local setpoint of the controller to other MAVs
-                (collision avoidance) and to the GCS.
-
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : Desired yaw angle (float)
-
-                '''
-                msg = MAVLink_local_position_setpoint_message(x, y, z, yaw)
-                msg.pack(self)
-                return msg
-            
-        def local_position_setpoint_send(self, x, y, z, yaw):
-                '''
-                Transmit the current local setpoint of the controller to other MAVs
-                (collision avoidance) and to the GCS.
-
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : Desired yaw angle (float)
-
-                '''
-                return self.send(self.local_position_setpoint_encode(x, y, z, yaw))
-            
-        def control_status_encode(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw):
-                '''
-                
-
-                position_fix              : Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix (uint8_t)
-                vision_fix                : Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix (uint8_t)
-                gps_fix                   : GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix (uint8_t)
-                ahrs_health               : Attitude estimation health: 0: poor, 255: excellent (uint8_t)
-                control_att               : 0: Attitude control disabled, 1: enabled (uint8_t)
-                control_pos_xy            : 0: X, Y position control disabled, 1: enabled (uint8_t)
-                control_pos_z             : 0: Z position control disabled, 1: enabled (uint8_t)
-                control_pos_yaw           : 0: Yaw angle control disabled, 1: enabled (uint8_t)
-
-                '''
-                msg = MAVLink_control_status_message(position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw)
-                msg.pack(self)
-                return msg
-            
-        def control_status_send(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw):
-                '''
-                
-
-                position_fix              : Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix (uint8_t)
-                vision_fix                : Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix (uint8_t)
-                gps_fix                   : GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix (uint8_t)
-                ahrs_health               : Attitude estimation health: 0: poor, 255: excellent (uint8_t)
-                control_att               : 0: Attitude control disabled, 1: enabled (uint8_t)
-                control_pos_xy            : 0: X, Y position control disabled, 1: enabled (uint8_t)
-                control_pos_z             : 0: Z position control disabled, 1: enabled (uint8_t)
-                control_pos_yaw           : 0: Yaw angle control disabled, 1: enabled (uint8_t)
-
-                '''
-                return self.send(self.control_status_encode(position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw))
-            
-        def safety_set_allowed_area_encode(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                '''
-                Set a safety zone (volume), which is defined by two corners of a cube.
-                This message can be used to tell the MAV which
-                setpoints/waypoints to accept and which to reject.
-                Safety areas are often enforced by national or
-                competition regulations.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                frame                     : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t)
-                p1x                       : x position 1 / Latitude 1 (float)
-                p1y                       : y position 1 / Longitude 1 (float)
-                p1z                       : z position 1 / Altitude 1 (float)
-                p2x                       : x position 2 / Latitude 2 (float)
-                p2y                       : y position 2 / Longitude 2 (float)
-                p2z                       : z position 2 / Altitude 2 (float)
-
-                '''
-                msg = MAVLink_safety_set_allowed_area_message(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z)
-                msg.pack(self)
-                return msg
-            
-        def safety_set_allowed_area_send(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                '''
-                Set a safety zone (volume), which is defined by two corners of a cube.
-                This message can be used to tell the MAV which
-                setpoints/waypoints to accept and which to reject.
-                Safety areas are often enforced by national or
-                competition regulations.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                frame                     : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t)
-                p1x                       : x position 1 / Latitude 1 (float)
-                p1y                       : y position 1 / Longitude 1 (float)
-                p1z                       : z position 1 / Altitude 1 (float)
-                p2x                       : x position 2 / Latitude 2 (float)
-                p2y                       : y position 2 / Longitude 2 (float)
-                p2z                       : z position 2 / Altitude 2 (float)
-
-                '''
-                return self.send(self.safety_set_allowed_area_encode(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z))
-            
-        def safety_allowed_area_encode(self, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                '''
-                Read out the safety zone the MAV currently assumes.
-
-                frame                     : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t)
-                p1x                       : x position 1 / Latitude 1 (float)
-                p1y                       : y position 1 / Longitude 1 (float)
-                p1z                       : z position 1 / Altitude 1 (float)
-                p2x                       : x position 2 / Latitude 2 (float)
-                p2y                       : y position 2 / Longitude 2 (float)
-                p2z                       : z position 2 / Altitude 2 (float)
-
-                '''
-                msg = MAVLink_safety_allowed_area_message(frame, p1x, p1y, p1z, p2x, p2y, p2z)
-                msg.pack(self)
-                return msg
-            
-        def safety_allowed_area_send(self, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                '''
-                Read out the safety zone the MAV currently assumes.
-
-                frame                     : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t)
-                p1x                       : x position 1 / Latitude 1 (float)
-                p1y                       : y position 1 / Longitude 1 (float)
-                p1z                       : z position 1 / Altitude 1 (float)
-                p2x                       : x position 2 / Latitude 2 (float)
-                p2y                       : y position 2 / Longitude 2 (float)
-                p2z                       : z position 2 / Altitude 2 (float)
-
-                '''
-                return self.send(self.safety_allowed_area_encode(frame, p1x, p1y, p1z, p2x, p2y, p2z))
-            
-        def set_roll_pitch_yaw_thrust_encode(self, target_system, target_component, roll, pitch, yaw, thrust):
-                '''
-                Set roll, pitch and yaw.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                roll                      : Desired roll angle in radians (float)
-                pitch                     : Desired pitch angle in radians (float)
-                yaw                       : Desired yaw angle in radians (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                msg = MAVLink_set_roll_pitch_yaw_thrust_message(target_system, target_component, roll, pitch, yaw, thrust)
-                msg.pack(self)
-                return msg
-            
-        def set_roll_pitch_yaw_thrust_send(self, target_system, target_component, roll, pitch, yaw, thrust):
-                '''
-                Set roll, pitch and yaw.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                roll                      : Desired roll angle in radians (float)
-                pitch                     : Desired pitch angle in radians (float)
-                yaw                       : Desired yaw angle in radians (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                return self.send(self.set_roll_pitch_yaw_thrust_encode(target_system, target_component, roll, pitch, yaw, thrust))
-            
-        def set_roll_pitch_yaw_speed_thrust_encode(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust):
-                '''
-                Set roll, pitch and yaw.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                roll_speed                : Desired roll angular speed in rad/s (float)
-                pitch_speed               : Desired pitch angular speed in rad/s (float)
-                yaw_speed                 : Desired yaw angular speed in rad/s (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                msg = MAVLink_set_roll_pitch_yaw_speed_thrust_message(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust)
-                msg.pack(self)
-                return msg
-            
-        def set_roll_pitch_yaw_speed_thrust_send(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust):
-                '''
-                Set roll, pitch and yaw.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                roll_speed                : Desired roll angular speed in rad/s (float)
-                pitch_speed               : Desired pitch angular speed in rad/s (float)
-                yaw_speed                 : Desired yaw angular speed in rad/s (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                return self.send(self.set_roll_pitch_yaw_speed_thrust_encode(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust))
-            
-        def roll_pitch_yaw_thrust_setpoint_encode(self, time_us, roll, pitch, yaw, thrust):
-                '''
-                Setpoint in roll, pitch, yaw currently active on the system.
-
-                time_us                   : Timestamp in micro seconds since unix epoch (uint64_t)
-                roll                      : Desired roll angle in radians (float)
-                pitch                     : Desired pitch angle in radians (float)
-                yaw                       : Desired yaw angle in radians (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                msg = MAVLink_roll_pitch_yaw_thrust_setpoint_message(time_us, roll, pitch, yaw, thrust)
-                msg.pack(self)
-                return msg
-            
-        def roll_pitch_yaw_thrust_setpoint_send(self, time_us, roll, pitch, yaw, thrust):
-                '''
-                Setpoint in roll, pitch, yaw currently active on the system.
-
-                time_us                   : Timestamp in micro seconds since unix epoch (uint64_t)
-                roll                      : Desired roll angle in radians (float)
-                pitch                     : Desired pitch angle in radians (float)
-                yaw                       : Desired yaw angle in radians (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                return self.send(self.roll_pitch_yaw_thrust_setpoint_encode(time_us, roll, pitch, yaw, thrust))
-            
-        def roll_pitch_yaw_speed_thrust_setpoint_encode(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust):
-                '''
-                Setpoint in rollspeed, pitchspeed, yawspeed currently active on the
-                system.
-
-                time_us                   : Timestamp in micro seconds since unix epoch (uint64_t)
-                roll_speed                : Desired roll angular speed in rad/s (float)
-                pitch_speed               : Desired pitch angular speed in rad/s (float)
-                yaw_speed                 : Desired yaw angular speed in rad/s (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                msg = MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(time_us, roll_speed, pitch_speed, yaw_speed, thrust)
-                msg.pack(self)
-                return msg
-            
-        def roll_pitch_yaw_speed_thrust_setpoint_send(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust):
-                '''
-                Setpoint in rollspeed, pitchspeed, yawspeed currently active on the
-                system.
-
-                time_us                   : Timestamp in micro seconds since unix epoch (uint64_t)
-                roll_speed                : Desired roll angular speed in rad/s (float)
-                pitch_speed               : Desired pitch angular speed in rad/s (float)
-                yaw_speed                 : Desired yaw angular speed in rad/s (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                return self.send(self.roll_pitch_yaw_speed_thrust_setpoint_encode(time_us, roll_speed, pitch_speed, yaw_speed, thrust))
-            
-        def nav_controller_output_encode(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error):
-                '''
-                Outputs of the APM navigation controller. The primary use of this
-                message is to check the response and signs of the
-                controller before actual flight and to assist with
-                tuning controller parameters
-
-                nav_roll                  : Current desired roll in degrees (float)
-                nav_pitch                 : Current desired pitch in degrees (float)
-                nav_bearing               : Current desired heading in degrees (int16_t)
-                target_bearing            : Bearing to current waypoint/target in degrees (int16_t)
-                wp_dist                   : Distance to active waypoint in meters (uint16_t)
-                alt_error                 : Current altitude error in meters (float)
-                aspd_error                : Current airspeed error in meters/second (float)
-                xtrack_error              : Current crosstrack error on x-y plane in meters (float)
-
-                '''
-                msg = MAVLink_nav_controller_output_message(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error)
-                msg.pack(self)
-                return msg
-            
-        def nav_controller_output_send(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error):
-                '''
-                Outputs of the APM navigation controller. The primary use of this
-                message is to check the response and signs of the
-                controller before actual flight and to assist with
-                tuning controller parameters
-
-                nav_roll                  : Current desired roll in degrees (float)
-                nav_pitch                 : Current desired pitch in degrees (float)
-                nav_bearing               : Current desired heading in degrees (int16_t)
-                target_bearing            : Bearing to current waypoint/target in degrees (int16_t)
-                wp_dist                   : Distance to active waypoint in meters (uint16_t)
-                alt_error                 : Current altitude error in meters (float)
-                aspd_error                : Current airspeed error in meters/second (float)
-                xtrack_error              : Current crosstrack error on x-y plane in meters (float)
-
-                '''
-                return self.send(self.nav_controller_output_encode(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error))
-            
-        def position_target_encode(self, x, y, z, yaw):
-                '''
-                The goal position of the system. This position is the input to any
-                navigation or path planning algorithm and does NOT
-                represent the current controller setpoint.
-
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : yaw orientation in radians, 0 = NORTH (float)
-
-                '''
-                msg = MAVLink_position_target_message(x, y, z, yaw)
-                msg.pack(self)
-                return msg
-            
-        def position_target_send(self, x, y, z, yaw):
-                '''
-                The goal position of the system. This position is the input to any
-                navigation or path planning algorithm and does NOT
-                represent the current controller setpoint.
-
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : yaw orientation in radians, 0 = NORTH (float)
-
-                '''
-                return self.send(self.position_target_encode(x, y, z, yaw))
-            
-        def state_correction_encode(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr):
-                '''
-                Corrects the systems state by adding an error correction term to the
-                position and velocity, and by rotating the attitude by
-                a correction angle.
-
-                xErr                      : x position error (float)
-                yErr                      : y position error (float)
-                zErr                      : z position error (float)
-                rollErr                   : roll error (radians) (float)
-                pitchErr                  : pitch error (radians) (float)
-                yawErr                    : yaw error (radians) (float)
-                vxErr                     : x velocity (float)
-                vyErr                     : y velocity (float)
-                vzErr                     : z velocity (float)
-
-                '''
-                msg = MAVLink_state_correction_message(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr)
-                msg.pack(self)
-                return msg
-            
-        def state_correction_send(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr):
-                '''
-                Corrects the systems state by adding an error correction term to the
-                position and velocity, and by rotating the attitude by
-                a correction angle.
-
-                xErr                      : x position error (float)
-                yErr                      : y position error (float)
-                zErr                      : z position error (float)
-                rollErr                   : roll error (radians) (float)
-                pitchErr                  : pitch error (radians) (float)
-                yawErr                    : yaw error (radians) (float)
-                vxErr                     : x velocity (float)
-                vyErr                     : y velocity (float)
-                vzErr                     : z velocity (float)
-
-                '''
-                return self.send(self.state_correction_encode(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr))
-            
-        def set_altitude_encode(self, target, mode):
-                '''
-                
-
-                target                    : The system setting the altitude (uint8_t)
-                mode                      : The new altitude in meters (uint32_t)
-
-                '''
-                msg = MAVLink_set_altitude_message(target, mode)
-                msg.pack(self)
-                return msg
-            
-        def set_altitude_send(self, target, mode):
-                '''
-                
-
-                target                    : The system setting the altitude (uint8_t)
-                mode                      : The new altitude in meters (uint32_t)
-
-                '''
-                return self.send(self.set_altitude_encode(target, mode))
-            
-        def request_data_stream_encode(self, target_system, target_component, req_stream_id, req_message_rate, start_stop):
-                '''
-                
-
-                target_system             : The target requested to send the message stream. (uint8_t)
-                target_component          : The target requested to send the message stream. (uint8_t)
-                req_stream_id             : The ID of the requested message type (uint8_t)
-                req_message_rate          : Update rate in Hertz (uint16_t)
-                start_stop                : 1 to start sending, 0 to stop sending. (uint8_t)
-
-                '''
-                msg = MAVLink_request_data_stream_message(target_system, target_component, req_stream_id, req_message_rate, start_stop)
-                msg.pack(self)
-                return msg
-            
-        def request_data_stream_send(self, target_system, target_component, req_stream_id, req_message_rate, start_stop):
-                '''
-                
-
-                target_system             : The target requested to send the message stream. (uint8_t)
-                target_component          : The target requested to send the message stream. (uint8_t)
-                req_stream_id             : The ID of the requested message type (uint8_t)
-                req_message_rate          : Update rate in Hertz (uint16_t)
-                start_stop                : 1 to start sending, 0 to stop sending. (uint8_t)
-
-                '''
-                return self.send(self.request_data_stream_encode(target_system, target_component, req_stream_id, req_message_rate, start_stop))
-            
-        def hil_state_encode(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc):
-                '''
-                This packet is useful for high throughput                 applications
-                such as hardware in the loop simulations.
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-                lat                       : Latitude, expressed as * 1E7 (int32_t)
-                lon                       : Longitude, expressed as * 1E7 (int32_t)
-                alt                       : Altitude in meters, expressed as * 1000 (millimeters) (int32_t)
-                vx                        : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t)
-                vy                        : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t)
-                vz                        : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t)
-                xacc                      : X acceleration (mg) (int16_t)
-                yacc                      : Y acceleration (mg) (int16_t)
-                zacc                      : Z acceleration (mg) (int16_t)
-
-                '''
-                msg = MAVLink_hil_state_message(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc)
-                msg.pack(self)
-                return msg
-            
-        def hil_state_send(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc):
-                '''
-                This packet is useful for high throughput                 applications
-                such as hardware in the loop simulations.
-
-                usec                      : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-                lat                       : Latitude, expressed as * 1E7 (int32_t)
-                lon                       : Longitude, expressed as * 1E7 (int32_t)
-                alt                       : Altitude in meters, expressed as * 1000 (millimeters) (int32_t)
-                vx                        : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t)
-                vy                        : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t)
-                vz                        : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t)
-                xacc                      : X acceleration (mg) (int16_t)
-                yacc                      : Y acceleration (mg) (int16_t)
-                zacc                      : Z acceleration (mg) (int16_t)
-
-                '''
-                return self.send(self.hil_state_encode(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc))
-            
-        def hil_controls_encode(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode):
-                '''
-                Hardware in the loop control outputs
-
-                time_us                   : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll_ailerons             : Control output -3 .. 1 (float)
-                pitch_elevator            : Control output -1 .. 1 (float)
-                yaw_rudder                : Control output -1 .. 1 (float)
-                throttle                  : Throttle 0 .. 1 (float)
-                mode                      : System mode (MAV_MODE) (uint8_t)
-                nav_mode                  : Navigation mode (MAV_NAV_MODE) (uint8_t)
-
-                '''
-                msg = MAVLink_hil_controls_message(time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode)
-                msg.pack(self)
-                return msg
-            
-        def hil_controls_send(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode):
-                '''
-                Hardware in the loop control outputs
-
-                time_us                   : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll_ailerons             : Control output -3 .. 1 (float)
-                pitch_elevator            : Control output -1 .. 1 (float)
-                yaw_rudder                : Control output -1 .. 1 (float)
-                throttle                  : Throttle 0 .. 1 (float)
-                mode                      : System mode (MAV_MODE) (uint8_t)
-                nav_mode                  : Navigation mode (MAV_NAV_MODE) (uint8_t)
-
-                '''
-                return self.send(self.hil_controls_encode(time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode))
-            
-        def manual_control_encode(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual):
-                '''
-                
-
-                target                    : The system to be controlled (uint8_t)
-                roll                      : roll (float)
-                pitch                     : pitch (float)
-                yaw                       : yaw (float)
-                thrust                    : thrust (float)
-                roll_manual               : roll control enabled auto:0, manual:1 (uint8_t)
-                pitch_manual              : pitch auto:0, manual:1 (uint8_t)
-                yaw_manual                : yaw auto:0, manual:1 (uint8_t)
-                thrust_manual             : thrust auto:0, manual:1 (uint8_t)
-
-                '''
-                msg = MAVLink_manual_control_message(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual)
-                msg.pack(self)
-                return msg
-            
-        def manual_control_send(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual):
-                '''
-                
-
-                target                    : The system to be controlled (uint8_t)
-                roll                      : roll (float)
-                pitch                     : pitch (float)
-                yaw                       : yaw (float)
-                thrust                    : thrust (float)
-                roll_manual               : roll control enabled auto:0, manual:1 (uint8_t)
-                pitch_manual              : pitch auto:0, manual:1 (uint8_t)
-                yaw_manual                : yaw auto:0, manual:1 (uint8_t)
-                thrust_manual             : thrust auto:0, manual:1 (uint8_t)
-
-                '''
-                return self.send(self.manual_control_encode(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual))
-            
-        def rc_channels_override_encode(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw):
-                '''
-                The RAW values of the RC channels sent to the MAV to override info
-                received from the RC radio. A value of -1 means no
-                change to that channel. A value of 0 means control of
-                that channel should be released back to the RC radio.
-                The standard PPM modulation is as follows: 1000
-                microseconds: 0%, 2000 microseconds: 100%. Individual
-                receivers/transmitters might violate this
-                specification.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-
-                '''
-                msg = MAVLink_rc_channels_override_message(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw)
-                msg.pack(self)
-                return msg
-            
-        def rc_channels_override_send(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw):
-                '''
-                The RAW values of the RC channels sent to the MAV to override info
-                received from the RC radio. A value of -1 means no
-                change to that channel. A value of 0 means control of
-                that channel should be released back to the RC radio.
-                The standard PPM modulation is as follows: 1000
-                microseconds: 0%, 2000 microseconds: 100%. Individual
-                receivers/transmitters might violate this
-                specification.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-
-                '''
-                return self.send(self.rc_channels_override_encode(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw))
-            
-        def global_position_int_encode(self, lat, lon, alt, vx, vy, vz):
-                '''
-                The filtered global position (e.g. fused GPS and accelerometers). The
-                position is in GPS-frame (right-handed, Z-up)
-
-                lat                       : Latitude, expressed as * 1E7 (int32_t)
-                lon                       : Longitude, expressed as * 1E7 (int32_t)
-                alt                       : Altitude in meters, expressed as * 1000 (millimeters) (int32_t)
-                vx                        : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t)
-                vy                        : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t)
-                vz                        : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t)
-
-                '''
-                msg = MAVLink_global_position_int_message(lat, lon, alt, vx, vy, vz)
-                msg.pack(self)
-                return msg
-            
-        def global_position_int_send(self, lat, lon, alt, vx, vy, vz):
-                '''
-                The filtered global position (e.g. fused GPS and accelerometers). The
-                position is in GPS-frame (right-handed, Z-up)
-
-                lat                       : Latitude, expressed as * 1E7 (int32_t)
-                lon                       : Longitude, expressed as * 1E7 (int32_t)
-                alt                       : Altitude in meters, expressed as * 1000 (millimeters) (int32_t)
-                vx                        : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t)
-                vy                        : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t)
-                vz                        : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t)
-
-                '''
-                return self.send(self.global_position_int_encode(lat, lon, alt, vx, vy, vz))
-            
-        def vfr_hud_encode(self, airspeed, groundspeed, heading, throttle, alt, climb):
-                '''
-                Metrics typically displayed on a HUD for fixed wing aircraft
-
-                airspeed                  : Current airspeed in m/s (float)
-                groundspeed               : Current ground speed in m/s (float)
-                heading                   : Current heading in degrees, in compass units (0..360, 0=north) (int16_t)
-                throttle                  : Current throttle setting in integer percent, 0 to 100 (uint16_t)
-                alt                       : Current altitude (MSL), in meters (float)
-                climb                     : Current climb rate in meters/second (float)
-
-                '''
-                msg = MAVLink_vfr_hud_message(airspeed, groundspeed, heading, throttle, alt, climb)
-                msg.pack(self)
-                return msg
-            
-        def vfr_hud_send(self, airspeed, groundspeed, heading, throttle, alt, climb):
-                '''
-                Metrics typically displayed on a HUD for fixed wing aircraft
-
-                airspeed                  : Current airspeed in m/s (float)
-                groundspeed               : Current ground speed in m/s (float)
-                heading                   : Current heading in degrees, in compass units (0..360, 0=north) (int16_t)
-                throttle                  : Current throttle setting in integer percent, 0 to 100 (uint16_t)
-                alt                       : Current altitude (MSL), in meters (float)
-                climb                     : Current climb rate in meters/second (float)
-
-                '''
-                return self.send(self.vfr_hud_encode(airspeed, groundspeed, heading, throttle, alt, climb))
-            
-        def command_encode(self, target_system, target_component, command, confirmation, param1, param2, param3, param4):
-                '''
-                Send a command with up to four parameters to the MAV
-
-                target_system             : System which should execute the command (uint8_t)
-                target_component          : Component which should execute the command, 0 for all components (uint8_t)
-                command                   : Command ID, as defined by MAV_CMD enum. (uint8_t)
-                confirmation              : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t)
-                param1                    : Parameter 1, as defined by MAV_CMD enum. (float)
-                param2                    : Parameter 2, as defined by MAV_CMD enum. (float)
-                param3                    : Parameter 3, as defined by MAV_CMD enum. (float)
-                param4                    : Parameter 4, as defined by MAV_CMD enum. (float)
-
-                '''
-                msg = MAVLink_command_message(target_system, target_component, command, confirmation, param1, param2, param3, param4)
-                msg.pack(self)
-                return msg
-            
-        def command_send(self, target_system, target_component, command, confirmation, param1, param2, param3, param4):
-                '''
-                Send a command with up to four parameters to the MAV
-
-                target_system             : System which should execute the command (uint8_t)
-                target_component          : Component which should execute the command, 0 for all components (uint8_t)
-                command                   : Command ID, as defined by MAV_CMD enum. (uint8_t)
-                confirmation              : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t)
-                param1                    : Parameter 1, as defined by MAV_CMD enum. (float)
-                param2                    : Parameter 2, as defined by MAV_CMD enum. (float)
-                param3                    : Parameter 3, as defined by MAV_CMD enum. (float)
-                param4                    : Parameter 4, as defined by MAV_CMD enum. (float)
-
-                '''
-                return self.send(self.command_encode(target_system, target_component, command, confirmation, param1, param2, param3, param4))
-            
-        def command_ack_encode(self, command, result):
-                '''
-                Report status of a command. Includes feedback wether the command was
-                executed
-
-                command                   : Current airspeed in m/s (float)
-                result                    : 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION (float)
-
-                '''
-                msg = MAVLink_command_ack_message(command, result)
-                msg.pack(self)
-                return msg
-            
-        def command_ack_send(self, command, result):
-                '''
-                Report status of a command. Includes feedback wether the command was
-                executed
-
-                command                   : Current airspeed in m/s (float)
-                result                    : 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION (float)
-
-                '''
-                return self.send(self.command_ack_encode(command, result))
-            
-        def optical_flow_encode(self, time, sensor_id, flow_x, flow_y, quality, ground_distance):
-                '''
-                Optical flow from a flow sensor (e.g. optical mouse sensor)
-
-                time                      : Timestamp (UNIX) (uint64_t)
-                sensor_id                 : Sensor ID (uint8_t)
-                flow_x                    : Flow in pixels in x-sensor direction (int16_t)
-                flow_y                    : Flow in pixels in y-sensor direction (int16_t)
-                quality                   : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t)
-                ground_distance           : Ground distance in meters (float)
-
-                '''
-                msg = MAVLink_optical_flow_message(time, sensor_id, flow_x, flow_y, quality, ground_distance)
-                msg.pack(self)
-                return msg
-            
-        def optical_flow_send(self, time, sensor_id, flow_x, flow_y, quality, ground_distance):
-                '''
-                Optical flow from a flow sensor (e.g. optical mouse sensor)
-
-                time                      : Timestamp (UNIX) (uint64_t)
-                sensor_id                 : Sensor ID (uint8_t)
-                flow_x                    : Flow in pixels in x-sensor direction (int16_t)
-                flow_y                    : Flow in pixels in y-sensor direction (int16_t)
-                quality                   : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t)
-                ground_distance           : Ground distance in meters (float)
-
-                '''
-                return self.send(self.optical_flow_encode(time, sensor_id, flow_x, flow_y, quality, ground_distance))
-            
-        def object_detection_event_encode(self, time, object_id, type, name, quality, bearing, distance):
-                '''
-                Object has been detected
-
-                time                      : Timestamp in milliseconds since system boot (uint32_t)
-                object_id                 : Object ID (uint16_t)
-                type                      : Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal (uint8_t)
-                name                      : Name of the object as defined by the detector (char)
-                quality                   : Detection quality / confidence. 0: bad, 255: maximum confidence (uint8_t)
-                bearing                   : Angle of the object with respect to the body frame in NED coordinates in radians. 0: front (float)
-                distance                  : Ground distance in meters (float)
-
-                '''
-                msg = MAVLink_object_detection_event_message(time, object_id, type, name, quality, bearing, distance)
-                msg.pack(self)
-                return msg
-            
-        def object_detection_event_send(self, time, object_id, type, name, quality, bearing, distance):
-                '''
-                Object has been detected
-
-                time                      : Timestamp in milliseconds since system boot (uint32_t)
-                object_id                 : Object ID (uint16_t)
-                type                      : Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal (uint8_t)
-                name                      : Name of the object as defined by the detector (char)
-                quality                   : Detection quality / confidence. 0: bad, 255: maximum confidence (uint8_t)
-                bearing                   : Angle of the object with respect to the body frame in NED coordinates in radians. 0: front (float)
-                distance                  : Ground distance in meters (float)
-
-                '''
-                return self.send(self.object_detection_event_encode(time, object_id, type, name, quality, bearing, distance))
-            
-        def debug_vect_encode(self, name, usec, x, y, z):
-                '''
-                
-
-                name                      : Name (char)
-                usec                      : Timestamp (uint64_t)
-                x                         : x (float)
-                y                         : y (float)
-                z                         : z (float)
-
-                '''
-                msg = MAVLink_debug_vect_message(name, usec, x, y, z)
-                msg.pack(self)
-                return msg
-            
-        def debug_vect_send(self, name, usec, x, y, z):
-                '''
-                
-
-                name                      : Name (char)
-                usec                      : Timestamp (uint64_t)
-                x                         : x (float)
-                y                         : y (float)
-                z                         : z (float)
-
-                '''
-                return self.send(self.debug_vect_encode(name, usec, x, y, z))
-            
-        def named_value_float_encode(self, name, value):
-                '''
-                Send a key-value pair as float. The use of this message is discouraged
-                for normal packets, but a quite efficient way for
-                testing new messages and getting experimental debug
-                output.
-
-                name                      : Name of the debug variable (char)
-                value                     : Floating point value (float)
-
-                '''
-                msg = MAVLink_named_value_float_message(name, value)
-                msg.pack(self)
-                return msg
-            
-        def named_value_float_send(self, name, value):
-                '''
-                Send a key-value pair as float. The use of this message is discouraged
-                for normal packets, but a quite efficient way for
-                testing new messages and getting experimental debug
-                output.
-
-                name                      : Name of the debug variable (char)
-                value                     : Floating point value (float)
-
-                '''
-                return self.send(self.named_value_float_encode(name, value))
-            
-        def named_value_int_encode(self, name, value):
-                '''
-                Send a key-value pair as integer. The use of this message is
-                discouraged for normal packets, but a quite efficient
-                way for testing new messages and getting experimental
-                debug output.
-
-                name                      : Name of the debug variable (char)
-                value                     : Signed integer value (int32_t)
-
-                '''
-                msg = MAVLink_named_value_int_message(name, value)
-                msg.pack(self)
-                return msg
-            
-        def named_value_int_send(self, name, value):
-                '''
-                Send a key-value pair as integer. The use of this message is
-                discouraged for normal packets, but a quite efficient
-                way for testing new messages and getting experimental
-                debug output.
-
-                name                      : Name of the debug variable (char)
-                value                     : Signed integer value (int32_t)
-
-                '''
-                return self.send(self.named_value_int_encode(name, value))
-            
-        def statustext_encode(self, severity, text):
-                '''
-                Status text message. These messages are printed in yellow in the COMM
-                console of QGroundControl. WARNING: They consume quite
-                some bandwidth, so use only for important status and
-                error messages. If implemented wisely, these messages
-                are buffered on the MCU and sent only at a limited
-                rate (e.g. 10 Hz).
-
-                severity                  : Severity of status, 0 = info message, 255 = critical fault (uint8_t)
-                text                      : Status text message, without null termination character (int8_t)
-
-                '''
-                msg = MAVLink_statustext_message(severity, text)
-                msg.pack(self)
-                return msg
-            
-        def statustext_send(self, severity, text):
-                '''
-                Status text message. These messages are printed in yellow in the COMM
-                console of QGroundControl. WARNING: They consume quite
-                some bandwidth, so use only for important status and
-                error messages. If implemented wisely, these messages
-                are buffered on the MCU and sent only at a limited
-                rate (e.g. 10 Hz).
-
-                severity                  : Severity of status, 0 = info message, 255 = critical fault (uint8_t)
-                text                      : Status text message, without null termination character (int8_t)
-
-                '''
-                return self.send(self.statustext_encode(severity, text))
-            
-        def debug_encode(self, ind, value):
-                '''
-                Send a debug value. The index is used to discriminate between values.
-                These values show up in the plot of QGroundControl as
-                DEBUG N.
-
-                ind                       : index of debug variable (uint8_t)
-                value                     : DEBUG value (float)
-
-                '''
-                msg = MAVLink_debug_message(ind, value)
-                msg.pack(self)
-                return msg
-            
-        def debug_send(self, ind, value):
-                '''
-                Send a debug value. The index is used to discriminate between values.
-                These values show up in the plot of QGroundControl as
-                DEBUG N.
-
-                ind                       : index of debug variable (uint8_t)
-                value                     : DEBUG value (float)
-
-                '''
-                return self.send(self.debug_encode(ind, value))
-            
diff --git a/mavlink/share/pyshared/pymavlink/mavlinkv10.py b/mavlink/share/pyshared/pymavlink/mavlinkv10.py
deleted file mode 100644
index a87e8e904..000000000
--- a/mavlink/share/pyshared/pymavlink/mavlinkv10.py
+++ /dev/null
@@ -1,5394 +0,0 @@
-'''
-MAVLink protocol implementation (auto-generated by mavgen.py)
-
-Generated from: ardupilotmega.xml,common.xml
-
-Note: this file has been auto-generated. DO NOT EDIT
-'''
-
-import struct, array, mavutil, time
-
-WIRE_PROTOCOL_VERSION = "1.0"
-
-class MAVLink_header(object):
-    '''MAVLink message header'''
-    def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
-        self.mlen = mlen
-        self.seq = seq
-        self.srcSystem = srcSystem
-        self.srcComponent = srcComponent
-        self.msgId = msgId
-
-    def pack(self):
-        return struct.pack('BBBBBB', 254, self.mlen, self.seq,
-                          self.srcSystem, self.srcComponent, self.msgId)
-
-class MAVLink_message(object):
-    '''base MAVLink message class'''
-    def __init__(self, msgId, name):
-        self._header     = MAVLink_header(msgId)
-        self._payload    = None
-        self._msgbuf     = None
-        self._crc        = None
-        self._fieldnames = []
-        self._type       = name
-
-    def get_msgbuf(self):
-        return self._msgbuf
-
-    def get_header(self):
-        return self._header
-
-    def get_payload(self):
-        return self._payload
-
-    def get_crc(self):
-        return self._crc
-
-    def get_fieldnames(self):
-        return self._fieldnames
-
-    def get_type(self):
-        return self._type
-
-    def get_msgId(self):
-        return self._header.msgId
-
-    def get_srcSystem(self):
-        return self._header.srcSystem
-
-    def get_srcComponent(self):
-        return self._header.srcComponent
-
-    def get_seq(self):
-        return self._header.seq
-
-    def __str__(self):
-        ret = '%s {' % self._type
-        for a in self._fieldnames:
-            v = getattr(self, a)
-            ret += '%s : %s, ' % (a, v)
-        ret = ret[0:-2] + '}'
-        return ret            
-
-    def pack(self, mav, crc_extra, payload):
-        self._payload = payload
-        self._header  = MAVLink_header(self._header.msgId, len(payload), mav.seq,
-                                       mav.srcSystem, mav.srcComponent)
-        self._msgbuf = self._header.pack() + payload
-        crc = mavutil.x25crc(self._msgbuf[1:])
-        if True: # using CRC extra
-            crc.accumulate(chr(crc_extra))
-        self._crc = crc.crc
-        self._msgbuf += struct.pack('<H', self._crc)
-        return self._msgbuf
-
-
-# enums
-
-# 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 # 
-
-# MAV_CMD
-MAV_CMD_NAV_WAYPOINT = 16 # Navigate to MISSION.
-MAV_CMD_NAV_LOITER_UNLIM = 17 # Loiter around this MISSION an unlimited amount of time
-MAV_CMD_NAV_LOITER_TURNS = 18 # Loiter around this MISSION for X turns
-MAV_CMD_NAV_LOITER_TIME = 19 # Loiter around this MISSION for X seconds
-MAV_CMD_NAV_RETURN_TO_LAUNCH = 20 # Return to launch location
-MAV_CMD_NAV_LAND = 21 # Land at location
-MAV_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand
-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.
-MAV_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV.
-MAV_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the
-                        # NAV/ACTION commands in the enumeration
-MAV_CMD_CONDITION_DELAY = 112 # Delay mission state machine.
-MAV_CMD_CONDITION_CHANGE_ALT = 113 # Ascend/descend at rate.  Delay mission state machine until desired
-                        # altitude reached.
-MAV_CMD_CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV
-                        # point.
-MAV_CMD_CONDITION_YAW = 115 # Reach a certain target angle.
-MAV_CMD_CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the
-                        # CONDITION commands in the enumeration
-MAV_CMD_DO_SET_MODE = 176 # Set system mode.
-MAV_CMD_DO_JUMP = 177 # Jump to the desired command in the mission list.  Repeat this action
-                        # only the specified number of times
-MAV_CMD_DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points.
-MAV_CMD_DO_SET_HOME = 179 # Changes the home location either to the current location or a
-                        # specified location.
-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.
-MAV_CMD_DO_SET_RELAY = 181 # Set a relay to a condition.
-MAV_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cyles with a desired
-                        # period.
-MAV_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value.
-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.
-MAV_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera system.
-MAV_CMD_DO_DIGICAM_CONFIGURE = 202 # Mission command to configure an on-board camera controller system.
-MAV_CMD_DO_DIGICAM_CONTROL = 203 # Mission command to control an on-board camera controller system.
-MAV_CMD_DO_MOUNT_CONFIGURE = 204 # Mission command to configure a camera or antenna mount
-MAV_CMD_DO_MOUNT_CONTROL = 205 # Mission command to control a camera or antenna mount
-MAV_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO
-                        # commands in the enumeration
-MAV_CMD_PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-
-                        # flight mode.
-MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-
-                        # flight mode.
-MAV_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command
-                        # will be only accepted if in pre-flight mode.
-MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components.
-MAV_CMD_OVERRIDE_GOTO = 252 # Hold / continue the current action
-MAV_CMD_MISSION_START = 300 # start running a mission
-MAV_CMD_ENUM_END = 301 # 
-
-# 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_ENUM_END = 2 # 
-
-# FENCE_BREACH
-FENCE_BREACH_NONE = 0 # No last fence breach
-FENCE_BREACH_MINALT = 1 # Breached minimum altitude
-FENCE_BREACH_MAXALT = 2 # Breached minimum altitude
-FENCE_BREACH_BOUNDARY = 3 # Breached fence boundary
-FENCE_BREACH_ENUM_END = 4 # 
-
-# MAV_AUTOPILOT
-MAV_AUTOPILOT_GENERIC = 0 # Generic autopilot, full support for everything
-MAV_AUTOPILOT_PIXHAWK = 1 # PIXHAWK autopilot, http://pixhawk.ethz.ch
-MAV_AUTOPILOT_SLUGS = 2 # SLUGS autopilot, http://slugsuav.soe.ucsc.edu
-MAV_AUTOPILOT_ARDUPILOTMEGA = 3 # ArduPilotMega / ArduCopter, http://diydrones.com
-MAV_AUTOPILOT_OPENPILOT = 4 # OpenPilot, http://openpilot.org
-MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY = 5 # Generic autopilot only supporting simple waypoints
-MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY = 6 # Generic autopilot supporting waypoints and other simple navigation
-                        # commands
-MAV_AUTOPILOT_GENERIC_MISSION_FULL = 7 # Generic autopilot supporting the full mission command set
-MAV_AUTOPILOT_INVALID = 8 # No valid autopilot, e.g. a GCS or other MAVLink component
-MAV_AUTOPILOT_PPZ = 9 # PPZ UAV - http://nongnu.org/paparazzi
-MAV_AUTOPILOT_UDB = 10 # UAV Dev Board
-MAV_AUTOPILOT_FP = 11 # FlexiPilot
-MAV_AUTOPILOT_ENUM_END = 12 # 
-
-# MAV_MODE_FLAG
-MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 # 0b00000001 Reserved for future use.
-MAV_MODE_FLAG_TEST_ENABLED = 2 # 0b00000010 system has a test mode enabled. This flag is intended for
-                        # temporary system tests and should not be
-                        # used for stable implementations.
-MAV_MODE_FLAG_AUTO_ENABLED = 4 # 0b00000100 autonomous mode enabled, system finds its own goal
-                        # positions. Guided flag can be set or not,
-                        # depends on the actual implementation.
-MAV_MODE_FLAG_GUIDED_ENABLED = 8 # 0b00001000 guided mode enabled, system flies MISSIONs / mission items.
-MAV_MODE_FLAG_STABILIZE_ENABLED = 16 # 0b00010000 system stabilizes electronically its attitude (and
-                        # optionally position). It needs however
-                        # further control inputs to move around.
-MAV_MODE_FLAG_HIL_ENABLED = 32 # 0b00100000 hardware in the loop simulation. All motors / actuators are
-                        # blocked, but internal software is full
-                        # operational.
-MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64 # 0b01000000 remote control input is enabled.
-MAV_MODE_FLAG_SAFETY_ARMED = 128 # 0b10000000 MAV safety set to armed. Motors are enabled / running / can
-                        # start. Ready to fly.
-MAV_MODE_FLAG_ENUM_END = 129 # 
-
-# MAV_MODE_FLAG_DECODE_POSITION
-MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE = 1 # Eighth bit: 00000001
-MAV_MODE_FLAG_DECODE_POSITION_TEST = 2 # Seventh bit: 00000010
-MAV_MODE_FLAG_DECODE_POSITION_AUTO = 4 # Sixt bit:   00000100
-MAV_MODE_FLAG_DECODE_POSITION_GUIDED = 8 # Fifth bit:  00001000
-MAV_MODE_FLAG_DECODE_POSITION_STABILIZE = 16 # Fourth bit: 00010000
-MAV_MODE_FLAG_DECODE_POSITION_HIL = 32 # Third bit:  00100000
-MAV_MODE_FLAG_DECODE_POSITION_MANUAL = 64 # Second bit: 01000000
-MAV_MODE_FLAG_DECODE_POSITION_SAFETY = 128 # First bit:  10000000
-MAV_MODE_FLAG_DECODE_POSITION_ENUM_END = 129 # 
-
-# MAV_GOTO
-MAV_GOTO_DO_HOLD = 0 # Hold at the current position.
-MAV_GOTO_DO_CONTINUE = 1 # Continue with the next item in mission execution.
-MAV_GOTO_HOLD_AT_CURRENT_POSITION = 2 # Hold at the current position of the system
-MAV_GOTO_HOLD_AT_SPECIFIED_POSITION = 3 # Hold at the position specified in the parameters of the DO_HOLD action
-MAV_GOTO_ENUM_END = 4 # 
-
-# MAV_MODE
-MAV_MODE_PREFLIGHT = 0 # System is not ready to fly, booting, calibrating, etc. No flag is set.
-MAV_MODE_MANUAL_DISARMED = 64 # System is allowed to be active, under manual (RC) control, no
-                        # stabilization
-MAV_MODE_TEST_DISARMED = 66 # UNDEFINED mode. This solely depends on the autopilot - use with
-                        # caution, intended for developers only.
-MAV_MODE_STABILIZE_DISARMED = 80 # System is allowed to be active, under assisted RC control.
-MAV_MODE_GUIDED_DISARMED = 88 # System is allowed to be active, under autonomous control, manual
-                        # setpoint
-MAV_MODE_AUTO_DISARMED = 92 # System is allowed to be active, under autonomous control and
-                        # navigation (the trajectory is decided
-                        # onboard and not pre-programmed by MISSIONs)
-MAV_MODE_MANUAL_ARMED = 192 # System is allowed to be active, under manual (RC) control, no
-                        # stabilization
-MAV_MODE_TEST_ARMED = 194 # UNDEFINED mode. This solely depends on the autopilot - use with
-                        # caution, intended for developers only.
-MAV_MODE_STABILIZE_ARMED = 208 # System is allowed to be active, under assisted RC control.
-MAV_MODE_GUIDED_ARMED = 216 # System is allowed to be active, under autonomous control, manual
-                        # setpoint
-MAV_MODE_AUTO_ARMED = 220 # System is allowed to be active, under autonomous control and
-                        # navigation (the trajectory is decided
-                        # onboard and not pre-programmed by MISSIONs)
-MAV_MODE_ENUM_END = 221 # 
-
-# MAV_STATE
-MAV_STATE_UNINIT = 0 # Uninitialized system, state is unknown.
-MAV_STATE_BOOT = 1 # System is booting up.
-MAV_STATE_CALIBRATING = 2 # System is calibrating and not flight-ready.
-MAV_STATE_STANDBY = 3 # System is grounded and on standby. It can be launched any time.
-MAV_STATE_ACTIVE = 4 # System is active and might be already airborne. Motors are engaged.
-MAV_STATE_CRITICAL = 5 # System is in a non-normal flight mode. It can however still navigate.
-MAV_STATE_EMERGENCY = 6 # System is in a non-normal flight mode. It lost control over parts or
-                        # over the whole airframe. It is in mayday and
-                        # going down.
-MAV_STATE_POWEROFF = 7 # System just initialized its power-down sequence, will shut down now.
-MAV_STATE_ENUM_END = 8 # 
-
-# MAV_TYPE
-MAV_TYPE_GENERIC = 0 # Generic micro air vehicle.
-MAV_TYPE_FIXED_WING = 1 # Fixed wing aircraft.
-MAV_TYPE_QUADROTOR = 2 # Quadrotor
-MAV_TYPE_COAXIAL = 3 # Coaxial helicopter
-MAV_TYPE_HELICOPTER = 4 # Normal helicopter with tail rotor.
-MAV_TYPE_ANTENNA_TRACKER = 5 # Ground installation
-MAV_TYPE_GCS = 6 # Operator control unit / ground control station
-MAV_TYPE_AIRSHIP = 7 # Airship, controlled
-MAV_TYPE_FREE_BALLOON = 8 # Free balloon, uncontrolled
-MAV_TYPE_ROCKET = 9 # Rocket
-MAV_TYPE_GROUND_ROVER = 10 # Ground rover
-MAV_TYPE_SURFACE_BOAT = 11 # Surface vessel, boat, ship
-MAV_TYPE_SUBMARINE = 12 # Submarine
-MAV_TYPE_HEXAROTOR = 13 # Hexarotor
-MAV_TYPE_OCTOROTOR = 14 # Octorotor
-MAV_TYPE_TRICOPTER = 15 # Octorotor
-MAV_TYPE_FLAPPING_WING = 16 # Flapping wing
-MAV_TYPE_ENUM_END = 17 # 
-
-# MAV_COMPONENT
-MAV_COMP_ID_ALL = 0 # 
-MAV_COMP_ID_CAMERA = 100 # 
-MAV_COMP_ID_SERVO1 = 140 # 
-MAV_COMP_ID_SERVO2 = 141 # 
-MAV_COMP_ID_SERVO3 = 142 # 
-MAV_COMP_ID_SERVO4 = 143 # 
-MAV_COMP_ID_SERVO5 = 144 # 
-MAV_COMP_ID_SERVO6 = 145 # 
-MAV_COMP_ID_SERVO7 = 146 # 
-MAV_COMP_ID_SERVO8 = 147 # 
-MAV_COMP_ID_SERVO9 = 148 # 
-MAV_COMP_ID_SERVO10 = 149 # 
-MAV_COMP_ID_SERVO11 = 150 # 
-MAV_COMP_ID_SERVO12 = 151 # 
-MAV_COMP_ID_SERVO13 = 152 # 
-MAV_COMP_ID_SERVO14 = 153 # 
-MAV_COMP_ID_MAPPER = 180 # 
-MAV_COMP_ID_MISSIONPLANNER = 190 # 
-MAV_COMP_ID_PATHPLANNER = 195 # 
-MAV_COMP_ID_IMU = 200 # 
-MAV_COMP_ID_IMU_2 = 201 # 
-MAV_COMP_ID_IMU_3 = 202 # 
-MAV_COMP_ID_GPS = 220 # 
-MAV_COMP_ID_UDP_BRIDGE = 240 # 
-MAV_COMP_ID_UART_BRIDGE = 241 # 
-MAV_COMP_ID_SYSTEM_CONTROL = 250 # 
-MAV_COMPONENT_ENUM_END = 251 # 
-
-# MAV_FRAME
-MAV_FRAME_GLOBAL = 0 # Global coordinate frame, WGS84 coordinate system. First value / x:
-                        # latitude, second value / y: longitude, third
-                        # value / z: positive altitude over mean sea
-                        # level (MSL)
-MAV_FRAME_LOCAL_NED = 1 # Local coordinate frame, Z-up (x: north, y: east, z: down).
-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 # 
-
-# MAVLINK_DATA_STREAM_TYPE
-MAVLINK_DATA_STREAM_IMG_JPEG = 1 # 
-MAVLINK_DATA_STREAM_IMG_BMP = 2 # 
-MAVLINK_DATA_STREAM_IMG_RAW8U = 3 # 
-MAVLINK_DATA_STREAM_IMG_RAW32U = 4 # 
-MAVLINK_DATA_STREAM_IMG_PGM = 5 # 
-MAVLINK_DATA_STREAM_IMG_PNG = 6 # 
-MAVLINK_DATA_STREAM_TYPE_ENUM_END = 7 # 
-
-# MAV_DATA_STREAM
-MAV_DATA_STREAM_ALL = 0 # Enable all data streams
-MAV_DATA_STREAM_RAW_SENSORS = 1 # Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
-MAV_DATA_STREAM_EXTENDED_STATUS = 2 # Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS
-MAV_DATA_STREAM_RC_CHANNELS = 3 # Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW
-MAV_DATA_STREAM_RAW_CONTROLLER = 4 # Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT,
-                        # NAV_CONTROLLER_OUTPUT.
-MAV_DATA_STREAM_POSITION = 6 # Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.
-MAV_DATA_STREAM_EXTRA1 = 10 # Dependent on the autopilot
-MAV_DATA_STREAM_EXTRA2 = 11 # Dependent on the autopilot
-MAV_DATA_STREAM_EXTRA3 = 12 # Dependent on the autopilot
-MAV_DATA_STREAM_ENUM_END = 13 # 
-
-# MAV_ROI
-MAV_ROI_NONE = 0 # No region of interest.
-MAV_ROI_WPNEXT = 1 # Point toward next MISSION.
-MAV_ROI_WPINDEX = 2 # Point toward given MISSION.
-MAV_ROI_LOCATION = 3 # Point toward fixed location.
-MAV_ROI_TARGET = 4 # Point toward of given id.
-MAV_ROI_ENUM_END = 5 # 
-
-# MAV_CMD_ACK
-MAV_CMD_ACK_OK = 1 # Command / mission item is ok.
-MAV_CMD_ACK_ERR_FAIL = 2 # Generic error message if none of the other reasons fails or if no
-                        # detailed error reporting is implemented.
-MAV_CMD_ACK_ERR_ACCESS_DENIED = 3 # The system is refusing to accept this command from this source /
-                        # communication partner.
-MAV_CMD_ACK_ERR_NOT_SUPPORTED = 4 # Command or mission item is not supported, other commands would be
-                        # accepted.
-MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED = 5 # The coordinate frame of this command / mission item is not supported.
-MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE = 6 # The coordinate frame of this command is ok, but he coordinate values
-                        # exceed the safety limits of this system.
-                        # This is a generic error, please use the more
-                        # specific error messages below if possible.
-MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE = 7 # The X or latitude value is out of range.
-MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE = 8 # The Y or longitude value is out of range.
-MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE = 9 # The Z or altitude value is out of range.
-MAV_CMD_ACK_ENUM_END = 10 # 
-
-# MAV_VAR
-MAV_VAR_FLOAT = 0 # 32 bit float
-MAV_VAR_UINT8 = 1 # 8 bit unsigned integer
-MAV_VAR_INT8 = 2 # 8 bit signed integer
-MAV_VAR_UINT16 = 3 # 16 bit unsigned integer
-MAV_VAR_INT16 = 4 # 16 bit signed integer
-MAV_VAR_UINT32 = 5 # 32 bit unsigned integer
-MAV_VAR_INT32 = 6 # 32 bit signed integer
-MAV_VAR_ENUM_END = 7 # 
-
-# MAV_RESULT
-MAV_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED
-MAV_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED
-MAV_RESULT_DENIED = 2 # Command PERMANENTLY DENIED
-MAV_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED
-MAV_RESULT_FAILED = 4 # Command executed, but failed
-MAV_RESULT_ENUM_END = 5 # 
-
-# MAV_MISSION_RESULT
-MAV_MISSION_ACCEPTED = 0 # mission accepted OK
-MAV_MISSION_ERROR = 1 # generic error / not accepting mission commands at all right now
-MAV_MISSION_UNSUPPORTED_FRAME = 2 # coordinate frame is not supported
-MAV_MISSION_UNSUPPORTED = 3 # command is not supported
-MAV_MISSION_NO_SPACE = 4 # mission item exceeds storage space
-MAV_MISSION_INVALID = 5 # one of the parameters has an invalid value
-MAV_MISSION_INVALID_PARAM1 = 6 # param1 has an invalid value
-MAV_MISSION_INVALID_PARAM2 = 7 # param2 has an invalid value
-MAV_MISSION_INVALID_PARAM3 = 8 # param3 has an invalid value
-MAV_MISSION_INVALID_PARAM4 = 9 # param4 has an invalid value
-MAV_MISSION_INVALID_PARAM5_X = 10 # x/param5 has an invalid value
-MAV_MISSION_INVALID_PARAM6_Y = 11 # y/param6 has an invalid value
-MAV_MISSION_INVALID_PARAM7 = 12 # param7 has an invalid value
-MAV_MISSION_INVALID_SEQUENCE = 13 # received waypoint out of sequence
-MAV_MISSION_DENIED = 14 # not accepting any mission commands from this communication partner
-MAV_MISSION_RESULT_ENUM_END = 15 # 
-
-# message IDs
-MAVLINK_MSG_ID_BAD_DATA = -1
-MAVLINK_MSG_ID_SENSOR_OFFSETS = 150
-MAVLINK_MSG_ID_SET_MAG_OFFSETS = 151
-MAVLINK_MSG_ID_MEMINFO = 152
-MAVLINK_MSG_ID_AP_ADC = 153
-MAVLINK_MSG_ID_DIGICAM_CONFIGURE = 154
-MAVLINK_MSG_ID_DIGICAM_CONTROL = 155
-MAVLINK_MSG_ID_MOUNT_CONFIGURE = 156
-MAVLINK_MSG_ID_MOUNT_CONTROL = 157
-MAVLINK_MSG_ID_MOUNT_STATUS = 158
-MAVLINK_MSG_ID_FENCE_POINT = 160
-MAVLINK_MSG_ID_FENCE_FETCH_POINT = 161
-MAVLINK_MSG_ID_FENCE_STATUS = 162
-MAVLINK_MSG_ID_AHRS = 163
-MAVLINK_MSG_ID_SIMSTATE = 164
-MAVLINK_MSG_ID_HWSTATUS = 165
-MAVLINK_MSG_ID_RADIO = 166
-MAVLINK_MSG_ID_HEARTBEAT = 0
-MAVLINK_MSG_ID_SYS_STATUS = 1
-MAVLINK_MSG_ID_SYSTEM_TIME = 2
-MAVLINK_MSG_ID_PING = 4
-MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL = 5
-MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK = 6
-MAVLINK_MSG_ID_AUTH_KEY = 7
-MAVLINK_MSG_ID_SET_MODE = 11
-MAVLINK_MSG_ID_PARAM_REQUEST_READ = 20
-MAVLINK_MSG_ID_PARAM_REQUEST_LIST = 21
-MAVLINK_MSG_ID_PARAM_VALUE = 22
-MAVLINK_MSG_ID_PARAM_SET = 23
-MAVLINK_MSG_ID_GPS_RAW_INT = 24
-MAVLINK_MSG_ID_GPS_STATUS = 25
-MAVLINK_MSG_ID_SCALED_IMU = 26
-MAVLINK_MSG_ID_RAW_IMU = 27
-MAVLINK_MSG_ID_RAW_PRESSURE = 28
-MAVLINK_MSG_ID_SCALED_PRESSURE = 29
-MAVLINK_MSG_ID_ATTITUDE = 30
-MAVLINK_MSG_ID_ATTITUDE_QUATERNION = 31
-MAVLINK_MSG_ID_LOCAL_POSITION_NED = 32
-MAVLINK_MSG_ID_GLOBAL_POSITION_INT = 33
-MAVLINK_MSG_ID_RC_CHANNELS_SCALED = 34
-MAVLINK_MSG_ID_RC_CHANNELS_RAW = 35
-MAVLINK_MSG_ID_SERVO_OUTPUT_RAW = 36
-MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST = 37
-MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST = 38
-MAVLINK_MSG_ID_MISSION_ITEM = 39
-MAVLINK_MSG_ID_MISSION_REQUEST = 40
-MAVLINK_MSG_ID_MISSION_SET_CURRENT = 41
-MAVLINK_MSG_ID_MISSION_CURRENT = 42
-MAVLINK_MSG_ID_MISSION_REQUEST_LIST = 43
-MAVLINK_MSG_ID_MISSION_COUNT = 44
-MAVLINK_MSG_ID_MISSION_CLEAR_ALL = 45
-MAVLINK_MSG_ID_MISSION_ITEM_REACHED = 46
-MAVLINK_MSG_ID_MISSION_ACK = 47
-MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN = 48
-MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN = 49
-MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT = 50
-MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT = 51
-MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT = 52
-MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT = 53
-MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA = 54
-MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA = 55
-MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST = 56
-MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST = 57
-MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT = 58
-MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT = 59
-MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT = 62
-MAVLINK_MSG_ID_STATE_CORRECTION = 64
-MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66
-MAVLINK_MSG_ID_DATA_STREAM = 67
-MAVLINK_MSG_ID_MANUAL_CONTROL = 69
-MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE = 70
-MAVLINK_MSG_ID_VFR_HUD = 74
-MAVLINK_MSG_ID_COMMAND_LONG = 76
-MAVLINK_MSG_ID_COMMAND_ACK = 77
-MAVLINK_MSG_ID_HIL_STATE = 90
-MAVLINK_MSG_ID_HIL_CONTROLS = 91
-MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW = 92
-MAVLINK_MSG_ID_OPTICAL_FLOW = 100
-MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE = 101
-MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE = 102
-MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE = 103
-MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE = 104
-MAVLINK_MSG_ID_MEMORY_VECT = 249
-MAVLINK_MSG_ID_DEBUG_VECT = 250
-MAVLINK_MSG_ID_NAMED_VALUE_FLOAT = 251
-MAVLINK_MSG_ID_NAMED_VALUE_INT = 252
-MAVLINK_MSG_ID_STATUSTEXT = 253
-MAVLINK_MSG_ID_DEBUG = 254
-MAVLINK_MSG_ID_EXTENDED_MESSAGE = 255
-
-class MAVLink_sensor_offsets_message(MAVLink_message):
-        '''
-        Offsets and calibrations values for hardware         sensors.
-        This makes it easier to debug the calibration process.
-        '''
-        def __init__(self, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SENSOR_OFFSETS, 'SENSOR_OFFSETS')
-                self._fieldnames = ['mag_ofs_x', 'mag_ofs_y', 'mag_ofs_z', 'mag_declination', 'raw_press', 'raw_temp', 'gyro_cal_x', 'gyro_cal_y', 'gyro_cal_z', 'accel_cal_x', 'accel_cal_y', 'accel_cal_z']
-                self.mag_ofs_x = mag_ofs_x
-                self.mag_ofs_y = mag_ofs_y
-                self.mag_ofs_z = mag_ofs_z
-                self.mag_declination = mag_declination
-                self.raw_press = raw_press
-                self.raw_temp = raw_temp
-                self.gyro_cal_x = gyro_cal_x
-                self.gyro_cal_y = gyro_cal_y
-                self.gyro_cal_z = gyro_cal_z
-                self.accel_cal_x = accel_cal_x
-                self.accel_cal_y = accel_cal_y
-                self.accel_cal_z = accel_cal_z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 134, struct.pack('<fiiffffffhhh', self.mag_declination, self.raw_press, self.raw_temp, self.gyro_cal_x, self.gyro_cal_y, self.gyro_cal_z, self.accel_cal_x, self.accel_cal_y, self.accel_cal_z, self.mag_ofs_x, self.mag_ofs_y, self.mag_ofs_z))
-
-class MAVLink_set_mag_offsets_message(MAVLink_message):
-        '''
-        set the magnetometer offsets
-        '''
-        def __init__(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_MAG_OFFSETS, 'SET_MAG_OFFSETS')
-                self._fieldnames = ['target_system', 'target_component', 'mag_ofs_x', 'mag_ofs_y', 'mag_ofs_z']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.mag_ofs_x = mag_ofs_x
-                self.mag_ofs_y = mag_ofs_y
-                self.mag_ofs_z = mag_ofs_z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 219, struct.pack('<hhhBB', self.mag_ofs_x, self.mag_ofs_y, self.mag_ofs_z, self.target_system, self.target_component))
-
-class MAVLink_meminfo_message(MAVLink_message):
-        '''
-        state of APM memory
-        '''
-        def __init__(self, brkval, freemem):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MEMINFO, 'MEMINFO')
-                self._fieldnames = ['brkval', 'freemem']
-                self.brkval = brkval
-                self.freemem = freemem
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 208, struct.pack('<HH', self.brkval, self.freemem))
-
-class MAVLink_ap_adc_message(MAVLink_message):
-        '''
-        raw ADC output
-        '''
-        def __init__(self, adc1, adc2, adc3, adc4, adc5, adc6):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_AP_ADC, 'AP_ADC')
-                self._fieldnames = ['adc1', 'adc2', 'adc3', 'adc4', 'adc5', 'adc6']
-                self.adc1 = adc1
-                self.adc2 = adc2
-                self.adc3 = adc3
-                self.adc4 = adc4
-                self.adc5 = adc5
-                self.adc6 = adc6
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 188, struct.pack('<HHHHHH', self.adc1, self.adc2, self.adc3, self.adc4, self.adc5, self.adc6))
-
-class MAVLink_digicam_configure_message(MAVLink_message):
-        '''
-        Configure on-board Camera Control System.
-        '''
-        def __init__(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, 'DIGICAM_CONFIGURE')
-                self._fieldnames = ['target_system', 'target_component', 'mode', 'shutter_speed', 'aperture', 'iso', 'exposure_type', 'command_id', 'engine_cut_off', 'extra_param', 'extra_value']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.mode = mode
-                self.shutter_speed = shutter_speed
-                self.aperture = aperture
-                self.iso = iso
-                self.exposure_type = exposure_type
-                self.command_id = command_id
-                self.engine_cut_off = engine_cut_off
-                self.extra_param = extra_param
-                self.extra_value = extra_value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 84, struct.pack('<fHBBBBBBBBB', self.extra_value, self.shutter_speed, self.target_system, self.target_component, self.mode, self.aperture, self.iso, self.exposure_type, self.command_id, self.engine_cut_off, self.extra_param))
-
-class MAVLink_digicam_control_message(MAVLink_message):
-        '''
-        Control on-board Camera Control System to take shots.
-        '''
-        def __init__(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DIGICAM_CONTROL, 'DIGICAM_CONTROL')
-                self._fieldnames = ['target_system', 'target_component', 'session', 'zoom_pos', 'zoom_step', 'focus_lock', 'shot', 'command_id', 'extra_param', 'extra_value']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.session = session
-                self.zoom_pos = zoom_pos
-                self.zoom_step = zoom_step
-                self.focus_lock = focus_lock
-                self.shot = shot
-                self.command_id = command_id
-                self.extra_param = extra_param
-                self.extra_value = extra_value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 22, struct.pack('<fBBBBbBBBB', self.extra_value, self.target_system, self.target_component, self.session, self.zoom_pos, self.zoom_step, self.focus_lock, self.shot, self.command_id, self.extra_param))
-
-class MAVLink_mount_configure_message(MAVLink_message):
-        '''
-        Message to configure a camera mount, directional antenna, etc.
-        '''
-        def __init__(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_CONFIGURE, 'MOUNT_CONFIGURE')
-                self._fieldnames = ['target_system', 'target_component', 'mount_mode', 'stab_roll', 'stab_pitch', 'stab_yaw']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.mount_mode = mount_mode
-                self.stab_roll = stab_roll
-                self.stab_pitch = stab_pitch
-                self.stab_yaw = stab_yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 19, struct.pack('<BBBBBB', self.target_system, self.target_component, self.mount_mode, self.stab_roll, self.stab_pitch, self.stab_yaw))
-
-class MAVLink_mount_control_message(MAVLink_message):
-        '''
-        Message to control a camera mount, directional antenna, etc.
-        '''
-        def __init__(self, target_system, target_component, input_a, input_b, input_c, save_position):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_CONTROL, 'MOUNT_CONTROL')
-                self._fieldnames = ['target_system', 'target_component', 'input_a', 'input_b', 'input_c', 'save_position']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.input_a = input_a
-                self.input_b = input_b
-                self.input_c = input_c
-                self.save_position = save_position
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 21, struct.pack('<iiiBBB', self.input_a, self.input_b, self.input_c, self.target_system, self.target_component, self.save_position))
-
-class MAVLink_mount_status_message(MAVLink_message):
-        '''
-        Message with some status from APM to GCS about camera or
-        antenna mount
-        '''
-        def __init__(self, target_system, target_component, pointing_a, pointing_b, pointing_c):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_STATUS, 'MOUNT_STATUS')
-                self._fieldnames = ['target_system', 'target_component', 'pointing_a', 'pointing_b', 'pointing_c']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.pointing_a = pointing_a
-                self.pointing_b = pointing_b
-                self.pointing_c = pointing_c
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 134, struct.pack('<iiiBB', self.pointing_a, self.pointing_b, self.pointing_c, self.target_system, self.target_component))
-
-class MAVLink_fence_point_message(MAVLink_message):
-        '''
-        A fence point. Used to set a point when from               GCS
-        -> MAV. Also used to return a point from MAV -> GCS
-        '''
-        def __init__(self, target_system, target_component, idx, count, lat, lng):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_POINT, 'FENCE_POINT')
-                self._fieldnames = ['target_system', 'target_component', 'idx', 'count', 'lat', 'lng']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.idx = idx
-                self.count = count
-                self.lat = lat
-                self.lng = lng
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 78, struct.pack('<ffBBBB', self.lat, self.lng, self.target_system, self.target_component, self.idx, self.count))
-
-class MAVLink_fence_fetch_point_message(MAVLink_message):
-        '''
-        Request a current fence point from MAV
-        '''
-        def __init__(self, target_system, target_component, idx):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_FETCH_POINT, 'FENCE_FETCH_POINT')
-                self._fieldnames = ['target_system', 'target_component', 'idx']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.idx = idx
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 68, struct.pack('<BBB', self.target_system, self.target_component, self.idx))
-
-class MAVLink_fence_status_message(MAVLink_message):
-        '''
-        Status of geo-fencing. Sent in extended             status
-        stream when fencing enabled
-        '''
-        def __init__(self, breach_status, breach_count, breach_type, breach_time):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_STATUS, 'FENCE_STATUS')
-                self._fieldnames = ['breach_status', 'breach_count', 'breach_type', 'breach_time']
-                self.breach_status = breach_status
-                self.breach_count = breach_count
-                self.breach_type = breach_type
-                self.breach_time = breach_time
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 189, struct.pack('<IHBB', self.breach_time, self.breach_count, self.breach_status, self.breach_type))
-
-class MAVLink_ahrs_message(MAVLink_message):
-        '''
-        Status of DCM attitude estimator
-        '''
-        def __init__(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_AHRS, 'AHRS')
-                self._fieldnames = ['omegaIx', 'omegaIy', 'omegaIz', 'accel_weight', 'renorm_val', 'error_rp', 'error_yaw']
-                self.omegaIx = omegaIx
-                self.omegaIy = omegaIy
-                self.omegaIz = omegaIz
-                self.accel_weight = accel_weight
-                self.renorm_val = renorm_val
-                self.error_rp = error_rp
-                self.error_yaw = error_yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 127, struct.pack('<fffffff', self.omegaIx, self.omegaIy, self.omegaIz, self.accel_weight, self.renorm_val, self.error_rp, self.error_yaw))
-
-class MAVLink_simstate_message(MAVLink_message):
-        '''
-        Status of simulation environment, if used
-        '''
-        def __init__(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SIMSTATE, 'SIMSTATE')
-                self._fieldnames = ['roll', 'pitch', 'yaw', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro']
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.xacc = xacc
-                self.yacc = yacc
-                self.zacc = zacc
-                self.xgyro = xgyro
-                self.ygyro = ygyro
-                self.zgyro = zgyro
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 42, struct.pack('<fffffffff', self.roll, self.pitch, self.yaw, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro))
-
-class MAVLink_hwstatus_message(MAVLink_message):
-        '''
-        Status of key hardware
-        '''
-        def __init__(self, Vcc, I2Cerr):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HWSTATUS, 'HWSTATUS')
-                self._fieldnames = ['Vcc', 'I2Cerr']
-                self.Vcc = Vcc
-                self.I2Cerr = I2Cerr
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 21, struct.pack('<HB', self.Vcc, self.I2Cerr))
-
-class MAVLink_radio_message(MAVLink_message):
-        '''
-        Status generated by radio
-        '''
-        def __init__(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RADIO, 'RADIO')
-                self._fieldnames = ['rssi', 'remrssi', 'txbuf', 'noise', 'remnoise', 'rxerrors', 'fixed']
-                self.rssi = rssi
-                self.remrssi = remrssi
-                self.txbuf = txbuf
-                self.noise = noise
-                self.remnoise = remnoise
-                self.rxerrors = rxerrors
-                self.fixed = fixed
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 21, struct.pack('<HHBBBBB', self.rxerrors, self.fixed, self.rssi, self.remrssi, self.txbuf, self.noise, self.remnoise))
-
-class MAVLink_heartbeat_message(MAVLink_message):
-        '''
-        The heartbeat message shows that a system is present and
-        responding. The type of the MAV and Autopilot hardware allow
-        the receiving system to treat further messages from this
-        system appropriate (e.g. by laying out the user interface
-        based on the autopilot).
-        '''
-        def __init__(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HEARTBEAT, 'HEARTBEAT')
-                self._fieldnames = ['type', 'autopilot', 'base_mode', 'custom_mode', 'system_status', 'mavlink_version']
-                self.type = type
-                self.autopilot = autopilot
-                self.base_mode = base_mode
-                self.custom_mode = custom_mode
-                self.system_status = system_status
-                self.mavlink_version = mavlink_version
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 50, struct.pack('<IBBBBB', self.custom_mode, self.type, self.autopilot, self.base_mode, self.system_status, self.mavlink_version))
-
-class MAVLink_sys_status_message(MAVLink_message):
-        '''
-        The general system state. If the system is following the
-        MAVLink standard, the system state is mainly defined by three
-        orthogonal states/modes: The system mode, which is either
-        LOCKED (motors shut down and locked), MANUAL (system under RC
-        control), GUIDED (system with autonomous position control,
-        position setpoint controlled manually) or AUTO (system guided
-        by path/waypoint planner). The NAV_MODE defined the current
-        flight state: LIFTOFF (often an open-loop maneuver), LANDING,
-        WAYPOINTS or VECTOR. This represents the internal navigation
-        state machine. The system status shows wether the system is
-        currently active or not and if an emergency occured. During
-        the CRITICAL and EMERGENCY states the MAV is still considered
-        to be active, but should start emergency procedures
-        autonomously. After a failure occured it should first move
-        from active to critical to allow manual intervention and then
-        move to emergency after a certain timeout.
-        '''
-        def __init__(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYS_STATUS, 'SYS_STATUS')
-                self._fieldnames = ['onboard_control_sensors_present', 'onboard_control_sensors_enabled', 'onboard_control_sensors_health', 'load', 'voltage_battery', 'current_battery', 'battery_remaining', 'drop_rate_comm', 'errors_comm', 'errors_count1', 'errors_count2', 'errors_count3', 'errors_count4']
-                self.onboard_control_sensors_present = onboard_control_sensors_present
-                self.onboard_control_sensors_enabled = onboard_control_sensors_enabled
-                self.onboard_control_sensors_health = onboard_control_sensors_health
-                self.load = load
-                self.voltage_battery = voltage_battery
-                self.current_battery = current_battery
-                self.battery_remaining = battery_remaining
-                self.drop_rate_comm = drop_rate_comm
-                self.errors_comm = errors_comm
-                self.errors_count1 = errors_count1
-                self.errors_count2 = errors_count2
-                self.errors_count3 = errors_count3
-                self.errors_count4 = errors_count4
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 124, struct.pack('<IIIHHhHHHHHHb', self.onboard_control_sensors_present, self.onboard_control_sensors_enabled, self.onboard_control_sensors_health, self.load, self.voltage_battery, self.current_battery, self.drop_rate_comm, self.errors_comm, self.errors_count1, self.errors_count2, self.errors_count3, self.errors_count4, self.battery_remaining))
-
-class MAVLink_system_time_message(MAVLink_message):
-        '''
-        The system time is the time of the master clock, typically the
-        computer clock of the main onboard computer.
-        '''
-        def __init__(self, time_unix_usec, time_boot_ms):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYSTEM_TIME, 'SYSTEM_TIME')
-                self._fieldnames = ['time_unix_usec', 'time_boot_ms']
-                self.time_unix_usec = time_unix_usec
-                self.time_boot_ms = time_boot_ms
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 137, struct.pack('<QI', self.time_unix_usec, self.time_boot_ms))
-
-class MAVLink_ping_message(MAVLink_message):
-        '''
-        A ping message either requesting or responding to a ping. This
-        allows to measure the system latencies, including serial port,
-        radio modem and UDP connections.
-        '''
-        def __init__(self, time_usec, seq, target_system, target_component):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PING, 'PING')
-                self._fieldnames = ['time_usec', 'seq', 'target_system', 'target_component']
-                self.time_usec = time_usec
-                self.seq = seq
-                self.target_system = target_system
-                self.target_component = target_component
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 237, struct.pack('<QIBB', self.time_usec, self.seq, self.target_system, self.target_component))
-
-class MAVLink_change_operator_control_message(MAVLink_message):
-        '''
-        Request to control this MAV
-        '''
-        def __init__(self, target_system, control_request, version, passkey):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, 'CHANGE_OPERATOR_CONTROL')
-                self._fieldnames = ['target_system', 'control_request', 'version', 'passkey']
-                self.target_system = target_system
-                self.control_request = control_request
-                self.version = version
-                self.passkey = passkey
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 217, struct.pack('<BBB25s', self.target_system, self.control_request, self.version, self.passkey))
-
-class MAVLink_change_operator_control_ack_message(MAVLink_message):
-        '''
-        Accept / deny control of this MAV
-        '''
-        def __init__(self, gcs_system_id, control_request, ack):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, 'CHANGE_OPERATOR_CONTROL_ACK')
-                self._fieldnames = ['gcs_system_id', 'control_request', 'ack']
-                self.gcs_system_id = gcs_system_id
-                self.control_request = control_request
-                self.ack = ack
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 104, struct.pack('<BBB', self.gcs_system_id, self.control_request, self.ack))
-
-class MAVLink_auth_key_message(MAVLink_message):
-        '''
-        Emit an encrypted signature / key identifying this system.
-        PLEASE NOTE: This protocol has been kept simple, so
-        transmitting the key requires an encrypted channel for true
-        safety.
-        '''
-        def __init__(self, key):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_AUTH_KEY, 'AUTH_KEY')
-                self._fieldnames = ['key']
-                self.key = key
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 119, struct.pack('<32s', self.key))
-
-class MAVLink_set_mode_message(MAVLink_message):
-        '''
-        Set the system mode, as defined by enum MAV_MODE. There is no
-        target component id as the mode is by definition for the
-        overall aircraft, not only for one component.
-        '''
-        def __init__(self, target_system, base_mode, custom_mode):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_MODE, 'SET_MODE')
-                self._fieldnames = ['target_system', 'base_mode', 'custom_mode']
-                self.target_system = target_system
-                self.base_mode = base_mode
-                self.custom_mode = custom_mode
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 89, struct.pack('<IBB', self.custom_mode, self.target_system, self.base_mode))
-
-class MAVLink_param_request_read_message(MAVLink_message):
-        '''
-        Request to read the onboard parameter with the param_id string
-        id. Onboard parameters are stored as key[const char*] ->
-        value[float]. This allows to send a parameter to any other
-        component (such as the GCS) without the need of previous
-        knowledge of possible parameter names. Thus the same GCS can
-        store different parameters for different autopilots. See also
-        http://qgroundcontrol.org/parameter_interface for a full
-        documentation of QGroundControl and IMU code.
-        '''
-        def __init__(self, target_system, target_component, param_id, param_index):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_READ, 'PARAM_REQUEST_READ')
-                self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_index']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.param_id = param_id
-                self.param_index = param_index
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 214, struct.pack('<hBB16s', self.param_index, self.target_system, self.target_component, self.param_id))
-
-class MAVLink_param_request_list_message(MAVLink_message):
-        '''
-        Request all parameters of this component. After his request,
-        all parameters are emitted.
-        '''
-        def __init__(self, target_system, target_component):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, 'PARAM_REQUEST_LIST')
-                self._fieldnames = ['target_system', 'target_component']
-                self.target_system = target_system
-                self.target_component = target_component
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 159, struct.pack('<BB', self.target_system, self.target_component))
-
-class MAVLink_param_value_message(MAVLink_message):
-        '''
-        Emit the value of a onboard parameter. The inclusion of
-        param_count and param_index in the message allows the
-        recipient to keep track of received parameters and allows him
-        to re-request missing parameters after a loss or timeout.
-        '''
-        def __init__(self, param_id, param_value, param_type, param_count, param_index):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_VALUE, 'PARAM_VALUE')
-                self._fieldnames = ['param_id', 'param_value', 'param_type', 'param_count', 'param_index']
-                self.param_id = param_id
-                self.param_value = param_value
-                self.param_type = param_type
-                self.param_count = param_count
-                self.param_index = param_index
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 220, struct.pack('<fHH16sB', self.param_value, self.param_count, self.param_index, self.param_id, self.param_type))
-
-class MAVLink_param_set_message(MAVLink_message):
-        '''
-        Set a parameter value TEMPORARILY to RAM. It will be reset to
-        default on system reboot. Send the ACTION
-        MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents
-        to EEPROM. IMPORTANT: The receiving component should
-        acknowledge the new parameter value by sending a param_value
-        message to all communication partners. This will also ensure
-        that multiple GCS all have an up-to-date list of all
-        parameters. If the sending GCS did not receive a PARAM_VALUE
-        message within its timeout time, it should re-send the
-        PARAM_SET message.
-        '''
-        def __init__(self, target_system, target_component, param_id, param_value, param_type):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_SET, 'PARAM_SET')
-                self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_value', 'param_type']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.param_id = param_id
-                self.param_value = param_value
-                self.param_type = param_type
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 168, struct.pack('<fBB16sB', self.param_value, self.target_system, self.target_component, self.param_id, self.param_type))
-
-class MAVLink_gps_raw_int_message(MAVLink_message):
-        '''
-        The global position, as returned by the Global Positioning
-        System (GPS). This is                 NOT the global position
-        estimate of the sytem, but rather a RAW sensor value. See
-        message GLOBAL_POSITION for the global position estimate.
-        Coordinate frame is right-handed, Z-axis up (GPS frame)
-        '''
-        def __init__(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_RAW_INT, 'GPS_RAW_INT')
-                self._fieldnames = ['time_usec', 'fix_type', 'lat', 'lon', 'alt', 'eph', 'epv', 'vel', 'cog', 'satellites_visible']
-                self.time_usec = time_usec
-                self.fix_type = fix_type
-                self.lat = lat
-                self.lon = lon
-                self.alt = alt
-                self.eph = eph
-                self.epv = epv
-                self.vel = vel
-                self.cog = cog
-                self.satellites_visible = satellites_visible
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 24, struct.pack('<QiiiHHHHBB', self.time_usec, self.lat, self.lon, self.alt, self.eph, self.epv, self.vel, self.cog, self.fix_type, self.satellites_visible))
-
-class MAVLink_gps_status_message(MAVLink_message):
-        '''
-        The positioning status, as reported by GPS. This message is
-        intended to display status information about each satellite
-        visible to the receiver. See message GLOBAL_POSITION for the
-        global position estimate. This message can contain information
-        for up to 20 satellites.
-        '''
-        def __init__(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_STATUS, 'GPS_STATUS')
-                self._fieldnames = ['satellites_visible', 'satellite_prn', 'satellite_used', 'satellite_elevation', 'satellite_azimuth', 'satellite_snr']
-                self.satellites_visible = satellites_visible
-                self.satellite_prn = satellite_prn
-                self.satellite_used = satellite_used
-                self.satellite_elevation = satellite_elevation
-                self.satellite_azimuth = satellite_azimuth
-                self.satellite_snr = satellite_snr
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 23, struct.pack('<B20s20s20s20s20s', self.satellites_visible, self.satellite_prn, self.satellite_used, self.satellite_elevation, self.satellite_azimuth, self.satellite_snr))
-
-class MAVLink_scaled_imu_message(MAVLink_message):
-        '''
-        The RAW IMU readings for the usual 9DOF sensor setup. This
-        message should contain the scaled values to the described
-        units
-        '''
-        def __init__(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SCALED_IMU, 'SCALED_IMU')
-                self._fieldnames = ['time_boot_ms', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro', 'xmag', 'ymag', 'zmag']
-                self.time_boot_ms = time_boot_ms
-                self.xacc = xacc
-                self.yacc = yacc
-                self.zacc = zacc
-                self.xgyro = xgyro
-                self.ygyro = ygyro
-                self.zgyro = zgyro
-                self.xmag = xmag
-                self.ymag = ymag
-                self.zmag = zmag
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 170, struct.pack('<Ihhhhhhhhh', self.time_boot_ms, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro, self.xmag, self.ymag, self.zmag))
-
-class MAVLink_raw_imu_message(MAVLink_message):
-        '''
-        The RAW IMU readings for the usual 9DOF sensor setup. This
-        message should always contain the true raw values without any
-        scaling to allow data capture and system debugging.
-        '''
-        def __init__(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RAW_IMU, 'RAW_IMU')
-                self._fieldnames = ['time_usec', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro', 'xmag', 'ymag', 'zmag']
-                self.time_usec = time_usec
-                self.xacc = xacc
-                self.yacc = yacc
-                self.zacc = zacc
-                self.xgyro = xgyro
-                self.ygyro = ygyro
-                self.zgyro = zgyro
-                self.xmag = xmag
-                self.ymag = ymag
-                self.zmag = zmag
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 144, struct.pack('<Qhhhhhhhhh', self.time_usec, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro, self.xmag, self.ymag, self.zmag))
-
-class MAVLink_raw_pressure_message(MAVLink_message):
-        '''
-        The RAW pressure readings for the typical setup of one
-        absolute pressure and one differential pressure sensor. The
-        sensor values should be the raw, UNSCALED ADC values.
-        '''
-        def __init__(self, time_usec, press_abs, press_diff1, press_diff2, temperature):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RAW_PRESSURE, 'RAW_PRESSURE')
-                self._fieldnames = ['time_usec', 'press_abs', 'press_diff1', 'press_diff2', 'temperature']
-                self.time_usec = time_usec
-                self.press_abs = press_abs
-                self.press_diff1 = press_diff1
-                self.press_diff2 = press_diff2
-                self.temperature = temperature
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 67, struct.pack('<Qhhhh', self.time_usec, self.press_abs, self.press_diff1, self.press_diff2, self.temperature))
-
-class MAVLink_scaled_pressure_message(MAVLink_message):
-        '''
-        The pressure readings for the typical setup of one absolute
-        and differential pressure sensor. The units are as specified
-        in each field.
-        '''
-        def __init__(self, time_boot_ms, press_abs, press_diff, temperature):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SCALED_PRESSURE, 'SCALED_PRESSURE')
-                self._fieldnames = ['time_boot_ms', 'press_abs', 'press_diff', 'temperature']
-                self.time_boot_ms = time_boot_ms
-                self.press_abs = press_abs
-                self.press_diff = press_diff
-                self.temperature = temperature
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 115, struct.pack('<Iffh', self.time_boot_ms, self.press_abs, self.press_diff, self.temperature))
-
-class MAVLink_attitude_message(MAVLink_message):
-        '''
-        The attitude in the aeronautical frame (right-handed, Z-down,
-        X-front, Y-right).
-        '''
-        def __init__(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ATTITUDE, 'ATTITUDE')
-                self._fieldnames = ['time_boot_ms', 'roll', 'pitch', 'yaw', 'rollspeed', 'pitchspeed', 'yawspeed']
-                self.time_boot_ms = time_boot_ms
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.rollspeed = rollspeed
-                self.pitchspeed = pitchspeed
-                self.yawspeed = yawspeed
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 39, struct.pack('<Iffffff', self.time_boot_ms, self.roll, self.pitch, self.yaw, self.rollspeed, self.pitchspeed, self.yawspeed))
-
-class MAVLink_attitude_quaternion_message(MAVLink_message):
-        '''
-        The attitude in the aeronautical frame (right-handed, Z-down,
-        X-front, Y-right), expressed as quaternion.
-        '''
-        def __init__(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, 'ATTITUDE_QUATERNION')
-                self._fieldnames = ['time_boot_ms', 'q1', 'q2', 'q3', 'q4', 'rollspeed', 'pitchspeed', 'yawspeed']
-                self.time_boot_ms = time_boot_ms
-                self.q1 = q1
-                self.q2 = q2
-                self.q3 = q3
-                self.q4 = q4
-                self.rollspeed = rollspeed
-                self.pitchspeed = pitchspeed
-                self.yawspeed = yawspeed
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 246, struct.pack('<Ifffffff', self.time_boot_ms, self.q1, self.q2, self.q3, self.q4, self.rollspeed, self.pitchspeed, self.yawspeed))
-
-class MAVLink_local_position_ned_message(MAVLink_message):
-        '''
-        The filtered local position (e.g. fused computer vision and
-        accelerometers). Coordinate frame is right-handed, Z-axis down
-        (aeronautical frame, NED / north-east-down convention)
-        '''
-        def __init__(self, time_boot_ms, x, y, z, vx, vy, vz):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION_NED, 'LOCAL_POSITION_NED')
-                self._fieldnames = ['time_boot_ms', 'x', 'y', 'z', 'vx', 'vy', 'vz']
-                self.time_boot_ms = time_boot_ms
-                self.x = x
-                self.y = y
-                self.z = z
-                self.vx = vx
-                self.vy = vy
-                self.vz = vz
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 185, struct.pack('<Iffffff', self.time_boot_ms, self.x, self.y, self.z, self.vx, self.vy, self.vz))
-
-class MAVLink_global_position_int_message(MAVLink_message):
-        '''
-        The filtered global position (e.g. fused GPS and
-        accelerometers). The position is in GPS-frame (right-handed,
-        Z-up). It                is designed as scaled integer message
-        since the resolution of float is not sufficient.
-        '''
-        def __init__(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, 'GLOBAL_POSITION_INT')
-                self._fieldnames = ['time_boot_ms', 'lat', 'lon', 'alt', 'relative_alt', 'vx', 'vy', 'vz', 'hdg']
-                self.time_boot_ms = time_boot_ms
-                self.lat = lat
-                self.lon = lon
-                self.alt = alt
-                self.relative_alt = relative_alt
-                self.vx = vx
-                self.vy = vy
-                self.vz = vz
-                self.hdg = hdg
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 104, struct.pack('<IiiiihhhH', self.time_boot_ms, self.lat, self.lon, self.alt, self.relative_alt, self.vx, self.vy, self.vz, self.hdg))
-
-class MAVLink_rc_channels_scaled_message(MAVLink_message):
-        '''
-        The scaled values of the RC channels received. (-100%) -10000,
-        (0%) 0, (100%) 10000
-        '''
-        def __init__(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, 'RC_CHANNELS_SCALED')
-                self._fieldnames = ['time_boot_ms', 'port', 'chan1_scaled', 'chan2_scaled', 'chan3_scaled', 'chan4_scaled', 'chan5_scaled', 'chan6_scaled', 'chan7_scaled', 'chan8_scaled', 'rssi']
-                self.time_boot_ms = time_boot_ms
-                self.port = port
-                self.chan1_scaled = chan1_scaled
-                self.chan2_scaled = chan2_scaled
-                self.chan3_scaled = chan3_scaled
-                self.chan4_scaled = chan4_scaled
-                self.chan5_scaled = chan5_scaled
-                self.chan6_scaled = chan6_scaled
-                self.chan7_scaled = chan7_scaled
-                self.chan8_scaled = chan8_scaled
-                self.rssi = rssi
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 237, struct.pack('<IhhhhhhhhBB', self.time_boot_ms, self.chan1_scaled, self.chan2_scaled, self.chan3_scaled, self.chan4_scaled, self.chan5_scaled, self.chan6_scaled, self.chan7_scaled, self.chan8_scaled, self.port, self.rssi))
-
-class MAVLink_rc_channels_raw_message(MAVLink_message):
-        '''
-        The RAW values of the RC channels received. The standard PPM
-        modulation is as follows: 1000 microseconds: 0%, 2000
-        microseconds: 100%. Individual receivers/transmitters might
-        violate this specification.
-        '''
-        def __init__(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_RAW, 'RC_CHANNELS_RAW')
-                self._fieldnames = ['time_boot_ms', 'port', 'chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw', 'rssi']
-                self.time_boot_ms = time_boot_ms
-                self.port = port
-                self.chan1_raw = chan1_raw
-                self.chan2_raw = chan2_raw
-                self.chan3_raw = chan3_raw
-                self.chan4_raw = chan4_raw
-                self.chan5_raw = chan5_raw
-                self.chan6_raw = chan6_raw
-                self.chan7_raw = chan7_raw
-                self.chan8_raw = chan8_raw
-                self.rssi = rssi
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 244, struct.pack('<IHHHHHHHHBB', self.time_boot_ms, self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw, self.port, self.rssi))
-
-class MAVLink_servo_output_raw_message(MAVLink_message):
-        '''
-        The RAW values of the servo outputs (for RC input from the
-        remote, use the RC_CHANNELS messages). The standard PPM
-        modulation is as follows: 1000 microseconds: 0%, 2000
-        microseconds: 100%.
-        '''
-        def __init__(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 'SERVO_OUTPUT_RAW')
-                self._fieldnames = ['time_usec', 'port', 'servo1_raw', 'servo2_raw', 'servo3_raw', 'servo4_raw', 'servo5_raw', 'servo6_raw', 'servo7_raw', 'servo8_raw']
-                self.time_usec = time_usec
-                self.port = port
-                self.servo1_raw = servo1_raw
-                self.servo2_raw = servo2_raw
-                self.servo3_raw = servo3_raw
-                self.servo4_raw = servo4_raw
-                self.servo5_raw = servo5_raw
-                self.servo6_raw = servo6_raw
-                self.servo7_raw = servo7_raw
-                self.servo8_raw = servo8_raw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 222, struct.pack('<IHHHHHHHHB', self.time_usec, self.servo1_raw, self.servo2_raw, self.servo3_raw, self.servo4_raw, self.servo5_raw, self.servo6_raw, self.servo7_raw, self.servo8_raw, self.port))
-
-class MAVLink_mission_request_partial_list_message(MAVLink_message):
-        '''
-        Request the overall list of MISSIONs from the
-        system/component.
-        http://qgroundcontrol.org/mavlink/waypoint_protocol
-        '''
-        def __init__(self, target_system, target_component, start_index, end_index):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, 'MISSION_REQUEST_PARTIAL_LIST')
-                self._fieldnames = ['target_system', 'target_component', 'start_index', 'end_index']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.start_index = start_index
-                self.end_index = end_index
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 212, struct.pack('<hhBB', self.start_index, self.end_index, self.target_system, self.target_component))
-
-class MAVLink_mission_write_partial_list_message(MAVLink_message):
-        '''
-        This message is sent to the MAV to write a partial list. If
-        start index == end index, only one item will be transmitted /
-        updated. If the start index is NOT 0 and above the current
-        list size, this request should be REJECTED!
-        '''
-        def __init__(self, target_system, target_component, start_index, end_index):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, 'MISSION_WRITE_PARTIAL_LIST')
-                self._fieldnames = ['target_system', 'target_component', 'start_index', 'end_index']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.start_index = start_index
-                self.end_index = end_index
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 9, struct.pack('<hhBB', self.start_index, self.end_index, self.target_system, self.target_component))
-
-class MAVLink_mission_item_message(MAVLink_message):
-        '''
-        Message encoding a mission item. This message is emitted to
-        announce                 the presence of a mission item and to
-        set a mission item on the system. The mission item can be
-        either in x, y, z meters (type: LOCAL) or x:lat, y:lon,
-        z:altitude. Local frame is Z-down, right handed (NED), global
-        frame is Z-up, right handed (ENU).
-        http://qgroundcontrol.org/mavlink/waypoint_protocol
-        '''
-        def __init__(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_ITEM, 'MISSION_ITEM')
-                self._fieldnames = ['target_system', 'target_component', 'seq', 'frame', 'command', 'current', 'autocontinue', 'param1', 'param2', 'param3', 'param4', 'x', 'y', 'z']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.seq = seq
-                self.frame = frame
-                self.command = command
-                self.current = current
-                self.autocontinue = autocontinue
-                self.param1 = param1
-                self.param2 = param2
-                self.param3 = param3
-                self.param4 = param4
-                self.x = x
-                self.y = y
-                self.z = z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 254, struct.pack('<fffffffHHBBBBB', self.param1, self.param2, self.param3, self.param4, self.x, self.y, self.z, self.seq, self.command, self.target_system, self.target_component, self.frame, self.current, self.autocontinue))
-
-class MAVLink_mission_request_message(MAVLink_message):
-        '''
-        Request the information of the mission item with the sequence
-        number seq. The response of the system to this message should
-        be a MISSION_ITEM message.
-        http://qgroundcontrol.org/mavlink/waypoint_protocol
-        '''
-        def __init__(self, target_system, target_component, seq):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_REQUEST, 'MISSION_REQUEST')
-                self._fieldnames = ['target_system', 'target_component', 'seq']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.seq = seq
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 230, struct.pack('<HBB', self.seq, self.target_system, self.target_component))
-
-class MAVLink_mission_set_current_message(MAVLink_message):
-        '''
-        Set the mission item with sequence number seq as current item.
-        This means that the MAV will continue to this mission item on
-        the shortest path (not following the mission items in-
-        between).
-        '''
-        def __init__(self, target_system, target_component, seq):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_SET_CURRENT, 'MISSION_SET_CURRENT')
-                self._fieldnames = ['target_system', 'target_component', 'seq']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.seq = seq
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 28, struct.pack('<HBB', self.seq, self.target_system, self.target_component))
-
-class MAVLink_mission_current_message(MAVLink_message):
-        '''
-        Message that announces the sequence number of the current
-        active mission item. The MAV will fly towards this mission
-        item.
-        '''
-        def __init__(self, seq):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_CURRENT, 'MISSION_CURRENT')
-                self._fieldnames = ['seq']
-                self.seq = seq
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 28, struct.pack('<H', self.seq))
-
-class MAVLink_mission_request_list_message(MAVLink_message):
-        '''
-        Request the overall list of mission items from the
-        system/component.
-        '''
-        def __init__(self, target_system, target_component):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, 'MISSION_REQUEST_LIST')
-                self._fieldnames = ['target_system', 'target_component']
-                self.target_system = target_system
-                self.target_component = target_component
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 132, struct.pack('<BB', self.target_system, self.target_component))
-
-class MAVLink_mission_count_message(MAVLink_message):
-        '''
-        This message is emitted as response to MISSION_REQUEST_LIST by
-        the MAV and to initiate a write transaction. The GCS can then
-        request the individual mission item based on the knowledge of
-        the total number of MISSIONs.
-        '''
-        def __init__(self, target_system, target_component, count):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_COUNT, 'MISSION_COUNT')
-                self._fieldnames = ['target_system', 'target_component', 'count']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.count = count
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 221, struct.pack('<HBB', self.count, self.target_system, self.target_component))
-
-class MAVLink_mission_clear_all_message(MAVLink_message):
-        '''
-        Delete all mission items at once.
-        '''
-        def __init__(self, target_system, target_component):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, 'MISSION_CLEAR_ALL')
-                self._fieldnames = ['target_system', 'target_component']
-                self.target_system = target_system
-                self.target_component = target_component
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 232, struct.pack('<BB', self.target_system, self.target_component))
-
-class MAVLink_mission_item_reached_message(MAVLink_message):
-        '''
-        A certain mission item has been reached. The system will
-        either hold this position (or circle on the orbit) or (if the
-        autocontinue on the WP was set) continue to the next MISSION.
-        '''
-        def __init__(self, seq):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, 'MISSION_ITEM_REACHED')
-                self._fieldnames = ['seq']
-                self.seq = seq
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 11, struct.pack('<H', self.seq))
-
-class MAVLink_mission_ack_message(MAVLink_message):
-        '''
-        Ack message during MISSION handling. The type field states if
-        this message is a positive ack (type=0) or if an error
-        happened (type=non-zero).
-        '''
-        def __init__(self, target_system, target_component, type):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MISSION_ACK, 'MISSION_ACK')
-                self._fieldnames = ['target_system', 'target_component', 'type']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.type = type
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 153, struct.pack('<BBB', self.target_system, self.target_component, self.type))
-
-class MAVLink_set_gps_global_origin_message(MAVLink_message):
-        '''
-        As local MISSIONs exist, the global MISSION reference allows
-        to transform between the local coordinate frame and the global
-        (GPS) coordinate frame. This can be necessary when e.g. in-
-        and outdoor settings are connected and the MAV should move
-        from in- to outdoor.
-        '''
-        def __init__(self, target_system, latitude, longitude, altitude):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, 'SET_GPS_GLOBAL_ORIGIN')
-                self._fieldnames = ['target_system', 'latitude', 'longitude', 'altitude']
-                self.target_system = target_system
-                self.latitude = latitude
-                self.longitude = longitude
-                self.altitude = altitude
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 41, struct.pack('<iiiB', self.latitude, self.longitude, self.altitude, self.target_system))
-
-class MAVLink_gps_global_origin_message(MAVLink_message):
-        '''
-        Once the MAV sets a new GPS-Local correspondence, this message
-        announces the origin (0,0,0) position
-        '''
-        def __init__(self, latitude, longitude, altitude):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, 'GPS_GLOBAL_ORIGIN')
-                self._fieldnames = ['latitude', 'longitude', 'altitude']
-                self.latitude = latitude
-                self.longitude = longitude
-                self.altitude = altitude
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 39, struct.pack('<iii', self.latitude, self.longitude, self.altitude))
-
-class MAVLink_set_local_position_setpoint_message(MAVLink_message):
-        '''
-        Set the setpoint for a local position controller. This is the
-        position in local coordinates the MAV should fly to. This
-        message is sent by the path/MISSION planner to the onboard
-        position controller. As some MAVs have a degree of freedom in
-        yaw (e.g. all helicopters/quadrotors), the desired yaw angle
-        is part of the message.
-        '''
-        def __init__(self, target_system, target_component, coordinate_frame, x, y, z, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, 'SET_LOCAL_POSITION_SETPOINT')
-                self._fieldnames = ['target_system', 'target_component', 'coordinate_frame', 'x', 'y', 'z', 'yaw']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.coordinate_frame = coordinate_frame
-                self.x = x
-                self.y = y
-                self.z = z
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 214, struct.pack('<ffffBBB', self.x, self.y, self.z, self.yaw, self.target_system, self.target_component, self.coordinate_frame))
-
-class MAVLink_local_position_setpoint_message(MAVLink_message):
-        '''
-        Transmit the current local setpoint of the controller to other
-        MAVs (collision avoidance) and to the GCS.
-        '''
-        def __init__(self, coordinate_frame, x, y, z, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, 'LOCAL_POSITION_SETPOINT')
-                self._fieldnames = ['coordinate_frame', 'x', 'y', 'z', 'yaw']
-                self.coordinate_frame = coordinate_frame
-                self.x = x
-                self.y = y
-                self.z = z
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 223, struct.pack('<ffffB', self.x, self.y, self.z, self.yaw, self.coordinate_frame))
-
-class MAVLink_global_position_setpoint_int_message(MAVLink_message):
-        '''
-        Transmit the current local setpoint of the controller to other
-        MAVs (collision avoidance) and to the GCS.
-        '''
-        def __init__(self, coordinate_frame, latitude, longitude, altitude, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, 'GLOBAL_POSITION_SETPOINT_INT')
-                self._fieldnames = ['coordinate_frame', 'latitude', 'longitude', 'altitude', 'yaw']
-                self.coordinate_frame = coordinate_frame
-                self.latitude = latitude
-                self.longitude = longitude
-                self.altitude = altitude
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 141, struct.pack('<iiihB', self.latitude, self.longitude, self.altitude, self.yaw, self.coordinate_frame))
-
-class MAVLink_set_global_position_setpoint_int_message(MAVLink_message):
-        '''
-        Set the current global position setpoint.
-        '''
-        def __init__(self, coordinate_frame, latitude, longitude, altitude, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, 'SET_GLOBAL_POSITION_SETPOINT_INT')
-                self._fieldnames = ['coordinate_frame', 'latitude', 'longitude', 'altitude', 'yaw']
-                self.coordinate_frame = coordinate_frame
-                self.latitude = latitude
-                self.longitude = longitude
-                self.altitude = altitude
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 33, struct.pack('<iiihB', self.latitude, self.longitude, self.altitude, self.yaw, self.coordinate_frame))
-
-class MAVLink_safety_set_allowed_area_message(MAVLink_message):
-        '''
-        Set a safety zone (volume), which is defined by two corners of
-        a cube. This message can be used to tell the MAV which
-        setpoints/MISSIONs to accept and which to reject. Safety areas
-        are often enforced by national or competition regulations.
-        '''
-        def __init__(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, 'SAFETY_SET_ALLOWED_AREA')
-                self._fieldnames = ['target_system', 'target_component', 'frame', 'p1x', 'p1y', 'p1z', 'p2x', 'p2y', 'p2z']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.frame = frame
-                self.p1x = p1x
-                self.p1y = p1y
-                self.p1z = p1z
-                self.p2x = p2x
-                self.p2y = p2y
-                self.p2z = p2z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 15, struct.pack('<ffffffBBB', self.p1x, self.p1y, self.p1z, self.p2x, self.p2y, self.p2z, self.target_system, self.target_component, self.frame))
-
-class MAVLink_safety_allowed_area_message(MAVLink_message):
-        '''
-        Read out the safety zone the MAV currently assumes.
-        '''
-        def __init__(self, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, 'SAFETY_ALLOWED_AREA')
-                self._fieldnames = ['frame', 'p1x', 'p1y', 'p1z', 'p2x', 'p2y', 'p2z']
-                self.frame = frame
-                self.p1x = p1x
-                self.p1y = p1y
-                self.p1z = p1z
-                self.p2x = p2x
-                self.p2y = p2y
-                self.p2z = p2z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 3, struct.pack('<ffffffB', self.p1x, self.p1y, self.p1z, self.p2x, self.p2y, self.p2z, self.frame))
-
-class MAVLink_set_roll_pitch_yaw_thrust_message(MAVLink_message):
-        '''
-        Set roll, pitch and yaw.
-        '''
-        def __init__(self, target_system, target_component, roll, pitch, yaw, thrust):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, 'SET_ROLL_PITCH_YAW_THRUST')
-                self._fieldnames = ['target_system', 'target_component', 'roll', 'pitch', 'yaw', 'thrust']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.thrust = thrust
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 100, struct.pack('<ffffBB', self.roll, self.pitch, self.yaw, self.thrust, self.target_system, self.target_component))
-
-class MAVLink_set_roll_pitch_yaw_speed_thrust_message(MAVLink_message):
-        '''
-        Set roll, pitch and yaw.
-        '''
-        def __init__(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, 'SET_ROLL_PITCH_YAW_SPEED_THRUST')
-                self._fieldnames = ['target_system', 'target_component', 'roll_speed', 'pitch_speed', 'yaw_speed', 'thrust']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.roll_speed = roll_speed
-                self.pitch_speed = pitch_speed
-                self.yaw_speed = yaw_speed
-                self.thrust = thrust
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 24, struct.pack('<ffffBB', self.roll_speed, self.pitch_speed, self.yaw_speed, self.thrust, self.target_system, self.target_component))
-
-class MAVLink_roll_pitch_yaw_thrust_setpoint_message(MAVLink_message):
-        '''
-        Setpoint in roll, pitch, yaw currently active on the system.
-        '''
-        def __init__(self, time_boot_ms, roll, pitch, yaw, thrust):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, 'ROLL_PITCH_YAW_THRUST_SETPOINT')
-                self._fieldnames = ['time_boot_ms', 'roll', 'pitch', 'yaw', 'thrust']
-                self.time_boot_ms = time_boot_ms
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.thrust = thrust
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 239, struct.pack('<Iffff', self.time_boot_ms, self.roll, self.pitch, self.yaw, self.thrust))
-
-class MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(MAVLink_message):
-        '''
-        Setpoint in rollspeed, pitchspeed, yawspeed currently active
-        on the system.
-        '''
-        def __init__(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, 'ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT')
-                self._fieldnames = ['time_boot_ms', 'roll_speed', 'pitch_speed', 'yaw_speed', 'thrust']
-                self.time_boot_ms = time_boot_ms
-                self.roll_speed = roll_speed
-                self.pitch_speed = pitch_speed
-                self.yaw_speed = yaw_speed
-                self.thrust = thrust
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 238, struct.pack('<Iffff', self.time_boot_ms, self.roll_speed, self.pitch_speed, self.yaw_speed, self.thrust))
-
-class MAVLink_nav_controller_output_message(MAVLink_message):
-        '''
-        Outputs of the APM navigation controller. The primary use of
-        this message is to check the response and signs
-        of the controller before actual flight and to assist with
-        tuning controller parameters
-        '''
-        def __init__(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, 'NAV_CONTROLLER_OUTPUT')
-                self._fieldnames = ['nav_roll', 'nav_pitch', 'nav_bearing', 'target_bearing', 'wp_dist', 'alt_error', 'aspd_error', 'xtrack_error']
-                self.nav_roll = nav_roll
-                self.nav_pitch = nav_pitch
-                self.nav_bearing = nav_bearing
-                self.target_bearing = target_bearing
-                self.wp_dist = wp_dist
-                self.alt_error = alt_error
-                self.aspd_error = aspd_error
-                self.xtrack_error = xtrack_error
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 183, struct.pack('<fffffhhH', self.nav_roll, self.nav_pitch, self.alt_error, self.aspd_error, self.xtrack_error, self.nav_bearing, self.target_bearing, self.wp_dist))
-
-class MAVLink_state_correction_message(MAVLink_message):
-        '''
-        Corrects the systems state by adding an error correction term
-        to the position and velocity, and by rotating the attitude by
-        a correction angle.
-        '''
-        def __init__(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_STATE_CORRECTION, 'STATE_CORRECTION')
-                self._fieldnames = ['xErr', 'yErr', 'zErr', 'rollErr', 'pitchErr', 'yawErr', 'vxErr', 'vyErr', 'vzErr']
-                self.xErr = xErr
-                self.yErr = yErr
-                self.zErr = zErr
-                self.rollErr = rollErr
-                self.pitchErr = pitchErr
-                self.yawErr = yawErr
-                self.vxErr = vxErr
-                self.vyErr = vyErr
-                self.vzErr = vzErr
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 130, struct.pack('<fffffffff', self.xErr, self.yErr, self.zErr, self.rollErr, self.pitchErr, self.yawErr, self.vxErr, self.vyErr, self.vzErr))
-
-class MAVLink_request_data_stream_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, target_system, target_component, req_stream_id, req_message_rate, start_stop):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, 'REQUEST_DATA_STREAM')
-                self._fieldnames = ['target_system', 'target_component', 'req_stream_id', 'req_message_rate', 'start_stop']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.req_stream_id = req_stream_id
-                self.req_message_rate = req_message_rate
-                self.start_stop = start_stop
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 148, struct.pack('<HBBBB', self.req_message_rate, self.target_system, self.target_component, self.req_stream_id, self.start_stop))
-
-class MAVLink_data_stream_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, stream_id, message_rate, on_off):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DATA_STREAM, 'DATA_STREAM')
-                self._fieldnames = ['stream_id', 'message_rate', 'on_off']
-                self.stream_id = stream_id
-                self.message_rate = message_rate
-                self.on_off = on_off
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 21, struct.pack('<HBB', self.message_rate, self.stream_id, self.on_off))
-
-class MAVLink_manual_control_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MANUAL_CONTROL, 'MANUAL_CONTROL')
-                self._fieldnames = ['target', 'roll', 'pitch', 'yaw', 'thrust', 'roll_manual', 'pitch_manual', 'yaw_manual', 'thrust_manual']
-                self.target = target
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.thrust = thrust
-                self.roll_manual = roll_manual
-                self.pitch_manual = pitch_manual
-                self.yaw_manual = yaw_manual
-                self.thrust_manual = thrust_manual
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 52, struct.pack('<ffffBBBBB', self.roll, self.pitch, self.yaw, self.thrust, self.target, self.roll_manual, self.pitch_manual, self.yaw_manual, self.thrust_manual))
-
-class MAVLink_rc_channels_override_message(MAVLink_message):
-        '''
-        The RAW values of the RC channels sent to the MAV to override
-        info received from the RC radio. A value of -1 means no change
-        to that channel. A value of 0 means control of that channel
-        should be released back to the RC radio. The standard PPM
-        modulation is as follows: 1000 microseconds: 0%, 2000
-        microseconds: 100%. Individual receivers/transmitters might
-        violate this specification.
-        '''
-        def __init__(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, 'RC_CHANNELS_OVERRIDE')
-                self._fieldnames = ['target_system', 'target_component', 'chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.chan1_raw = chan1_raw
-                self.chan2_raw = chan2_raw
-                self.chan3_raw = chan3_raw
-                self.chan4_raw = chan4_raw
-                self.chan5_raw = chan5_raw
-                self.chan6_raw = chan6_raw
-                self.chan7_raw = chan7_raw
-                self.chan8_raw = chan8_raw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 124, struct.pack('<HHHHHHHHBB', self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw, self.target_system, self.target_component))
-
-class MAVLink_vfr_hud_message(MAVLink_message):
-        '''
-        Metrics typically displayed on a HUD for fixed wing aircraft
-        '''
-        def __init__(self, airspeed, groundspeed, heading, throttle, alt, climb):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_VFR_HUD, 'VFR_HUD')
-                self._fieldnames = ['airspeed', 'groundspeed', 'heading', 'throttle', 'alt', 'climb']
-                self.airspeed = airspeed
-                self.groundspeed = groundspeed
-                self.heading = heading
-                self.throttle = throttle
-                self.alt = alt
-                self.climb = climb
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 20, struct.pack('<ffffhH', self.airspeed, self.groundspeed, self.alt, self.climb, self.heading, self.throttle))
-
-class MAVLink_command_long_message(MAVLink_message):
-        '''
-        Send a command with up to four parameters to the MAV
-        '''
-        def __init__(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_COMMAND_LONG, 'COMMAND_LONG')
-                self._fieldnames = ['target_system', 'target_component', 'command', 'confirmation', 'param1', 'param2', 'param3', 'param4', 'param5', 'param6', 'param7']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.command = command
-                self.confirmation = confirmation
-                self.param1 = param1
-                self.param2 = param2
-                self.param3 = param3
-                self.param4 = param4
-                self.param5 = param5
-                self.param6 = param6
-                self.param7 = param7
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 152, struct.pack('<fffffffHBBB', self.param1, self.param2, self.param3, self.param4, self.param5, self.param6, self.param7, self.command, self.target_system, self.target_component, self.confirmation))
-
-class MAVLink_command_ack_message(MAVLink_message):
-        '''
-        Report status of a command. Includes feedback wether the
-        command was executed
-        '''
-        def __init__(self, command, result):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_COMMAND_ACK, 'COMMAND_ACK')
-                self._fieldnames = ['command', 'result']
-                self.command = command
-                self.result = result
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 143, struct.pack('<HB', self.command, self.result))
-
-class MAVLink_hil_state_message(MAVLink_message):
-        '''
-        Sent from simulation to autopilot. This packet is useful for
-        high throughput                 applications such as hardware
-        in the loop simulations.
-        '''
-        def __init__(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_STATE, 'HIL_STATE')
-                self._fieldnames = ['time_usec', 'roll', 'pitch', 'yaw', 'rollspeed', 'pitchspeed', 'yawspeed', 'lat', 'lon', 'alt', 'vx', 'vy', 'vz', 'xacc', 'yacc', 'zacc']
-                self.time_usec = time_usec
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-                self.rollspeed = rollspeed
-                self.pitchspeed = pitchspeed
-                self.yawspeed = yawspeed
-                self.lat = lat
-                self.lon = lon
-                self.alt = alt
-                self.vx = vx
-                self.vy = vy
-                self.vz = vz
-                self.xacc = xacc
-                self.yacc = yacc
-                self.zacc = zacc
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 183, struct.pack('<Qffffffiiihhhhhh', self.time_usec, self.roll, self.pitch, self.yaw, self.rollspeed, self.pitchspeed, self.yawspeed, self.lat, self.lon, self.alt, self.vx, self.vy, self.vz, self.xacc, self.yacc, self.zacc))
-
-class MAVLink_hil_controls_message(MAVLink_message):
-        '''
-        Sent from autopilot to simulation. Hardware in the loop
-        control outputs
-        '''
-        def __init__(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_CONTROLS, 'HIL_CONTROLS')
-                self._fieldnames = ['time_usec', 'roll_ailerons', 'pitch_elevator', 'yaw_rudder', 'throttle', 'aux1', 'aux2', 'aux3', 'aux4', 'mode', 'nav_mode']
-                self.time_usec = time_usec
-                self.roll_ailerons = roll_ailerons
-                self.pitch_elevator = pitch_elevator
-                self.yaw_rudder = yaw_rudder
-                self.throttle = throttle
-                self.aux1 = aux1
-                self.aux2 = aux2
-                self.aux3 = aux3
-                self.aux4 = aux4
-                self.mode = mode
-                self.nav_mode = nav_mode
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 63, struct.pack('<QffffffffBB', self.time_usec, self.roll_ailerons, self.pitch_elevator, self.yaw_rudder, self.throttle, self.aux1, self.aux2, self.aux3, self.aux4, self.mode, self.nav_mode))
-
-class MAVLink_hil_rc_inputs_raw_message(MAVLink_message):
-        '''
-        Sent from simulation to autopilot. The RAW values of the RC
-        channels received. The standard PPM modulation is as follows:
-        1000 microseconds: 0%, 2000 microseconds: 100%. Individual
-        receivers/transmitters might violate this specification.
-        '''
-        def __init__(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, 'HIL_RC_INPUTS_RAW')
-                self._fieldnames = ['time_usec', 'chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw', 'chan9_raw', 'chan10_raw', 'chan11_raw', 'chan12_raw', 'rssi']
-                self.time_usec = time_usec
-                self.chan1_raw = chan1_raw
-                self.chan2_raw = chan2_raw
-                self.chan3_raw = chan3_raw
-                self.chan4_raw = chan4_raw
-                self.chan5_raw = chan5_raw
-                self.chan6_raw = chan6_raw
-                self.chan7_raw = chan7_raw
-                self.chan8_raw = chan8_raw
-                self.chan9_raw = chan9_raw
-                self.chan10_raw = chan10_raw
-                self.chan11_raw = chan11_raw
-                self.chan12_raw = chan12_raw
-                self.rssi = rssi
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 54, struct.pack('<QHHHHHHHHHHHHB', self.time_usec, self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw, self.chan9_raw, self.chan10_raw, self.chan11_raw, self.chan12_raw, self.rssi))
-
-class MAVLink_optical_flow_message(MAVLink_message):
-        '''
-        Optical flow from a flow sensor (e.g. optical mouse sensor)
-        '''
-        def __init__(self, time_usec, sensor_id, flow_x, flow_y, quality, ground_distance):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_OPTICAL_FLOW, 'OPTICAL_FLOW')
-                self._fieldnames = ['time_usec', 'sensor_id', 'flow_x', 'flow_y', 'quality', 'ground_distance']
-                self.time_usec = time_usec
-                self.sensor_id = sensor_id
-                self.flow_x = flow_x
-                self.flow_y = flow_y
-                self.quality = quality
-                self.ground_distance = ground_distance
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 19, struct.pack('<QfhhBB', self.time_usec, self.ground_distance, self.flow_x, self.flow_y, self.sensor_id, self.quality))
-
-class MAVLink_global_vision_position_estimate_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, usec, x, y, z, roll, pitch, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, 'GLOBAL_VISION_POSITION_ESTIMATE')
-                self._fieldnames = ['usec', 'x', 'y', 'z', 'roll', 'pitch', 'yaw']
-                self.usec = usec
-                self.x = x
-                self.y = y
-                self.z = z
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 102, struct.pack('<Qffffff', self.usec, self.x, self.y, self.z, self.roll, self.pitch, self.yaw))
-
-class MAVLink_vision_position_estimate_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, usec, x, y, z, roll, pitch, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, 'VISION_POSITION_ESTIMATE')
-                self._fieldnames = ['usec', 'x', 'y', 'z', 'roll', 'pitch', 'yaw']
-                self.usec = usec
-                self.x = x
-                self.y = y
-                self.z = z
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 158, struct.pack('<Qffffff', self.usec, self.x, self.y, self.z, self.roll, self.pitch, self.yaw))
-
-class MAVLink_vision_speed_estimate_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, usec, x, y, z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, 'VISION_SPEED_ESTIMATE')
-                self._fieldnames = ['usec', 'x', 'y', 'z']
-                self.usec = usec
-                self.x = x
-                self.y = y
-                self.z = z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 208, struct.pack('<Qfff', self.usec, self.x, self.y, self.z))
-
-class MAVLink_vicon_position_estimate_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, usec, x, y, z, roll, pitch, yaw):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, 'VICON_POSITION_ESTIMATE')
-                self._fieldnames = ['usec', 'x', 'y', 'z', 'roll', 'pitch', 'yaw']
-                self.usec = usec
-                self.x = x
-                self.y = y
-                self.z = z
-                self.roll = roll
-                self.pitch = pitch
-                self.yaw = yaw
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 56, struct.pack('<Qffffff', self.usec, self.x, self.y, self.z, self.roll, self.pitch, self.yaw))
-
-class MAVLink_memory_vect_message(MAVLink_message):
-        '''
-        Send raw controller memory. The use of this message is
-        discouraged for normal packets, but a quite efficient way for
-        testing new messages and getting experimental debug output.
-        '''
-        def __init__(self, address, ver, type, value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_MEMORY_VECT, 'MEMORY_VECT')
-                self._fieldnames = ['address', 'ver', 'type', 'value']
-                self.address = address
-                self.ver = ver
-                self.type = type
-                self.value = value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 204, struct.pack('<HBB32s', self.address, self.ver, self.type, self.value))
-
-class MAVLink_debug_vect_message(MAVLink_message):
-        '''
-
-        '''
-        def __init__(self, name, time_usec, x, y, z):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DEBUG_VECT, 'DEBUG_VECT')
-                self._fieldnames = ['name', 'time_usec', 'x', 'y', 'z']
-                self.name = name
-                self.time_usec = time_usec
-                self.x = x
-                self.y = y
-                self.z = z
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 49, struct.pack('<Qfff10s', self.time_usec, self.x, self.y, self.z, self.name))
-
-class MAVLink_named_value_float_message(MAVLink_message):
-        '''
-        Send a key-value pair as float. The use of this message is
-        discouraged for normal packets, but a quite efficient way for
-        testing new messages and getting experimental debug output.
-        '''
-        def __init__(self, time_boot_ms, name, value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 'NAMED_VALUE_FLOAT')
-                self._fieldnames = ['time_boot_ms', 'name', 'value']
-                self.time_boot_ms = time_boot_ms
-                self.name = name
-                self.value = value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 170, struct.pack('<If10s', self.time_boot_ms, self.value, self.name))
-
-class MAVLink_named_value_int_message(MAVLink_message):
-        '''
-        Send a key-value pair as integer. The use of this message is
-        discouraged for normal packets, but a quite efficient way for
-        testing new messages and getting experimental debug output.
-        '''
-        def __init__(self, time_boot_ms, name, value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAMED_VALUE_INT, 'NAMED_VALUE_INT')
-                self._fieldnames = ['time_boot_ms', 'name', 'value']
-                self.time_boot_ms = time_boot_ms
-                self.name = name
-                self.value = value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 44, struct.pack('<Ii10s', self.time_boot_ms, self.value, self.name))
-
-class MAVLink_statustext_message(MAVLink_message):
-        '''
-        Status text message. These messages are printed in yellow in
-        the COMM console of QGroundControl. WARNING: They consume
-        quite some bandwidth, so use only for important status and
-        error messages. If implemented wisely, these messages are
-        buffered on the MCU and sent only at a limited rate (e.g. 10
-        Hz).
-        '''
-        def __init__(self, severity, text):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_STATUSTEXT, 'STATUSTEXT')
-                self._fieldnames = ['severity', 'text']
-                self.severity = severity
-                self.text = text
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 83, struct.pack('<B50s', self.severity, self.text))
-
-class MAVLink_debug_message(MAVLink_message):
-        '''
-        Send a debug value. The index is used to discriminate between
-        values. These values show up in the plot of QGroundControl as
-        DEBUG N.
-        '''
-        def __init__(self, time_boot_ms, ind, value):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_DEBUG, 'DEBUG')
-                self._fieldnames = ['time_boot_ms', 'ind', 'value']
-                self.time_boot_ms = time_boot_ms
-                self.ind = ind
-                self.value = value
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 46, struct.pack('<IfB', self.time_boot_ms, self.value, self.ind))
-
-class MAVLink_extended_message_message(MAVLink_message):
-        '''
-        Extended message spacer.
-        '''
-        def __init__(self, target_system, target_component, protocol_flags):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_EXTENDED_MESSAGE, 'EXTENDED_MESSAGE')
-                self._fieldnames = ['target_system', 'target_component', 'protocol_flags']
-                self.target_system = target_system
-                self.target_component = target_component
-                self.protocol_flags = protocol_flags
-
-        def pack(self, mav):
-                return MAVLink_message.pack(self, mav, 247, struct.pack('<BBB', self.target_system, self.target_component, self.protocol_flags))
-
-
-mavlink_map = {
-        MAVLINK_MSG_ID_SENSOR_OFFSETS : ( '<fiiffffffhhh', MAVLink_sensor_offsets_message, [9, 10, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8], 134 ),
-        MAVLINK_MSG_ID_SET_MAG_OFFSETS : ( '<hhhBB', MAVLink_set_mag_offsets_message, [3, 4, 0, 1, 2], 219 ),
-        MAVLINK_MSG_ID_MEMINFO : ( '<HH', MAVLink_meminfo_message, [0, 1], 208 ),
-        MAVLINK_MSG_ID_AP_ADC : ( '<HHHHHH', MAVLink_ap_adc_message, [0, 1, 2, 3, 4, 5], 188 ),
-        MAVLINK_MSG_ID_DIGICAM_CONFIGURE : ( '<fHBBBBBBBBB', MAVLink_digicam_configure_message, [2, 3, 4, 1, 5, 6, 7, 8, 9, 10, 0], 84 ),
-        MAVLINK_MSG_ID_DIGICAM_CONTROL : ( '<fBBBBbBBBB', MAVLink_digicam_control_message, [1, 2, 3, 4, 5, 6, 7, 8, 9, 0], 22 ),
-        MAVLINK_MSG_ID_MOUNT_CONFIGURE : ( '<BBBBBB', MAVLink_mount_configure_message, [0, 1, 2, 3, 4, 5], 19 ),
-        MAVLINK_MSG_ID_MOUNT_CONTROL : ( '<iiiBBB', MAVLink_mount_control_message, [3, 4, 0, 1, 2, 5], 21 ),
-        MAVLINK_MSG_ID_MOUNT_STATUS : ( '<iiiBB', MAVLink_mount_status_message, [3, 4, 0, 1, 2], 134 ),
-        MAVLINK_MSG_ID_FENCE_POINT : ( '<ffBBBB', MAVLink_fence_point_message, [2, 3, 4, 5, 0, 1], 78 ),
-        MAVLINK_MSG_ID_FENCE_FETCH_POINT : ( '<BBB', MAVLink_fence_fetch_point_message, [0, 1, 2], 68 ),
-        MAVLINK_MSG_ID_FENCE_STATUS : ( '<IHBB', MAVLink_fence_status_message, [2, 1, 3, 0], 189 ),
-        MAVLINK_MSG_ID_AHRS : ( '<fffffff', MAVLink_ahrs_message, [0, 1, 2, 3, 4, 5, 6], 127 ),
-        MAVLINK_MSG_ID_SIMSTATE : ( '<fffffffff', MAVLink_simstate_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 42 ),
-        MAVLINK_MSG_ID_HWSTATUS : ( '<HB', MAVLink_hwstatus_message, [0, 1], 21 ),
-        MAVLINK_MSG_ID_RADIO : ( '<HHBBBBB', MAVLink_radio_message, [2, 3, 4, 5, 6, 0, 1], 21 ),
-        MAVLINK_MSG_ID_HEARTBEAT : ( '<IBBBBB', MAVLink_heartbeat_message, [1, 2, 3, 0, 4, 5], 50 ),
-        MAVLINK_MSG_ID_SYS_STATUS : ( '<IIIHHhHHHHHHb', MAVLink_sys_status_message, [0, 1, 2, 3, 4, 5, 12, 6, 7, 8, 9, 10, 11], 124 ),
-        MAVLINK_MSG_ID_SYSTEM_TIME : ( '<QI', MAVLink_system_time_message, [0, 1], 137 ),
-        MAVLINK_MSG_ID_PING : ( '<QIBB', MAVLink_ping_message, [0, 1, 2, 3], 237 ),
-        MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL : ( '<BBB25s', MAVLink_change_operator_control_message, [0, 1, 2, 3], 217 ),
-        MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK : ( '<BBB', MAVLink_change_operator_control_ack_message, [0, 1, 2], 104 ),
-        MAVLINK_MSG_ID_AUTH_KEY : ( '<32s', MAVLink_auth_key_message, [0], 119 ),
-        MAVLINK_MSG_ID_SET_MODE : ( '<IBB', MAVLink_set_mode_message, [1, 2, 0], 89 ),
-        MAVLINK_MSG_ID_PARAM_REQUEST_READ : ( '<hBB16s', MAVLink_param_request_read_message, [1, 2, 3, 0], 214 ),
-        MAVLINK_MSG_ID_PARAM_REQUEST_LIST : ( '<BB', MAVLink_param_request_list_message, [0, 1], 159 ),
-        MAVLINK_MSG_ID_PARAM_VALUE : ( '<fHH16sB', MAVLink_param_value_message, [3, 0, 4, 1, 2], 220 ),
-        MAVLINK_MSG_ID_PARAM_SET : ( '<fBB16sB', MAVLink_param_set_message, [1, 2, 3, 0, 4], 168 ),
-        MAVLINK_MSG_ID_GPS_RAW_INT : ( '<QiiiHHHHBB', MAVLink_gps_raw_int_message, [0, 8, 1, 2, 3, 4, 5, 6, 7, 9], 24 ),
-        MAVLINK_MSG_ID_GPS_STATUS : ( '<B20s20s20s20s20s', MAVLink_gps_status_message, [0, 1, 2, 3, 4, 5], 23 ),
-        MAVLINK_MSG_ID_SCALED_IMU : ( '<Ihhhhhhhhh', MAVLink_scaled_imu_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 170 ),
-        MAVLINK_MSG_ID_RAW_IMU : ( '<Qhhhhhhhhh', MAVLink_raw_imu_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 144 ),
-        MAVLINK_MSG_ID_RAW_PRESSURE : ( '<Qhhhh', MAVLink_raw_pressure_message, [0, 1, 2, 3, 4], 67 ),
-        MAVLINK_MSG_ID_SCALED_PRESSURE : ( '<Iffh', MAVLink_scaled_pressure_message, [0, 1, 2, 3], 115 ),
-        MAVLINK_MSG_ID_ATTITUDE : ( '<Iffffff', MAVLink_attitude_message, [0, 1, 2, 3, 4, 5, 6], 39 ),
-        MAVLINK_MSG_ID_ATTITUDE_QUATERNION : ( '<Ifffffff', MAVLink_attitude_quaternion_message, [0, 1, 2, 3, 4, 5, 6, 7], 246 ),
-        MAVLINK_MSG_ID_LOCAL_POSITION_NED : ( '<Iffffff', MAVLink_local_position_ned_message, [0, 1, 2, 3, 4, 5, 6], 185 ),
-        MAVLINK_MSG_ID_GLOBAL_POSITION_INT : ( '<IiiiihhhH', MAVLink_global_position_int_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 104 ),
-        MAVLINK_MSG_ID_RC_CHANNELS_SCALED : ( '<IhhhhhhhhBB', MAVLink_rc_channels_scaled_message, [0, 9, 1, 2, 3, 4, 5, 6, 7, 8, 10], 237 ),
-        MAVLINK_MSG_ID_RC_CHANNELS_RAW : ( '<IHHHHHHHHBB', MAVLink_rc_channels_raw_message, [0, 9, 1, 2, 3, 4, 5, 6, 7, 8, 10], 244 ),
-        MAVLINK_MSG_ID_SERVO_OUTPUT_RAW : ( '<IHHHHHHHHB', MAVLink_servo_output_raw_message, [0, 9, 1, 2, 3, 4, 5, 6, 7, 8], 222 ),
-        MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST : ( '<hhBB', MAVLink_mission_request_partial_list_message, [2, 3, 0, 1], 212 ),
-        MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST : ( '<hhBB', MAVLink_mission_write_partial_list_message, [2, 3, 0, 1], 9 ),
-        MAVLINK_MSG_ID_MISSION_ITEM : ( '<fffffffHHBBBBB', MAVLink_mission_item_message, [9, 10, 7, 11, 8, 12, 13, 0, 1, 2, 3, 4, 5, 6], 254 ),
-        MAVLINK_MSG_ID_MISSION_REQUEST : ( '<HBB', MAVLink_mission_request_message, [1, 2, 0], 230 ),
-        MAVLINK_MSG_ID_MISSION_SET_CURRENT : ( '<HBB', MAVLink_mission_set_current_message, [1, 2, 0], 28 ),
-        MAVLINK_MSG_ID_MISSION_CURRENT : ( '<H', MAVLink_mission_current_message, [0], 28 ),
-        MAVLINK_MSG_ID_MISSION_REQUEST_LIST : ( '<BB', MAVLink_mission_request_list_message, [0, 1], 132 ),
-        MAVLINK_MSG_ID_MISSION_COUNT : ( '<HBB', MAVLink_mission_count_message, [1, 2, 0], 221 ),
-        MAVLINK_MSG_ID_MISSION_CLEAR_ALL : ( '<BB', MAVLink_mission_clear_all_message, [0, 1], 232 ),
-        MAVLINK_MSG_ID_MISSION_ITEM_REACHED : ( '<H', MAVLink_mission_item_reached_message, [0], 11 ),
-        MAVLINK_MSG_ID_MISSION_ACK : ( '<BBB', MAVLink_mission_ack_message, [0, 1, 2], 153 ),
-        MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN : ( '<iiiB', MAVLink_set_gps_global_origin_message, [3, 0, 1, 2], 41 ),
-        MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN : ( '<iii', MAVLink_gps_global_origin_message, [0, 1, 2], 39 ),
-        MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT : ( '<ffffBBB', MAVLink_set_local_position_setpoint_message, [4, 5, 6, 0, 1, 2, 3], 214 ),
-        MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT : ( '<ffffB', MAVLink_local_position_setpoint_message, [4, 0, 1, 2, 3], 223 ),
-        MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT : ( '<iiihB', MAVLink_global_position_setpoint_int_message, [4, 0, 1, 2, 3], 141 ),
-        MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT : ( '<iiihB', MAVLink_set_global_position_setpoint_int_message, [4, 0, 1, 2, 3], 33 ),
-        MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA : ( '<ffffffBBB', MAVLink_safety_set_allowed_area_message, [6, 7, 8, 0, 1, 2, 3, 4, 5], 15 ),
-        MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA : ( '<ffffffB', MAVLink_safety_allowed_area_message, [6, 0, 1, 2, 3, 4, 5], 3 ),
-        MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST : ( '<ffffBB', MAVLink_set_roll_pitch_yaw_thrust_message, [4, 5, 0, 1, 2, 3], 100 ),
-        MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST : ( '<ffffBB', MAVLink_set_roll_pitch_yaw_speed_thrust_message, [4, 5, 0, 1, 2, 3], 24 ),
-        MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT : ( '<Iffff', MAVLink_roll_pitch_yaw_thrust_setpoint_message, [0, 1, 2, 3, 4], 239 ),
-        MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT : ( '<Iffff', MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message, [0, 1, 2, 3, 4], 238 ),
-        MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT : ( '<fffffhhH', MAVLink_nav_controller_output_message, [0, 1, 5, 6, 7, 2, 3, 4], 183 ),
-        MAVLINK_MSG_ID_STATE_CORRECTION : ( '<fffffffff', MAVLink_state_correction_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 130 ),
-        MAVLINK_MSG_ID_REQUEST_DATA_STREAM : ( '<HBBBB', MAVLink_request_data_stream_message, [1, 2, 3, 0, 4], 148 ),
-        MAVLINK_MSG_ID_DATA_STREAM : ( '<HBB', MAVLink_data_stream_message, [1, 0, 2], 21 ),
-        MAVLINK_MSG_ID_MANUAL_CONTROL : ( '<ffffBBBBB', MAVLink_manual_control_message, [4, 0, 1, 2, 3, 5, 6, 7, 8], 52 ),
-        MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE : ( '<HHHHHHHHBB', MAVLink_rc_channels_override_message, [8, 9, 0, 1, 2, 3, 4, 5, 6, 7], 124 ),
-        MAVLINK_MSG_ID_VFR_HUD : ( '<ffffhH', MAVLink_vfr_hud_message, [0, 1, 4, 5, 2, 3], 20 ),
-        MAVLINK_MSG_ID_COMMAND_LONG : ( '<fffffffHBBB', MAVLink_command_long_message, [8, 9, 7, 10, 0, 1, 2, 3, 4, 5, 6], 152 ),
-        MAVLINK_MSG_ID_COMMAND_ACK : ( '<HB', MAVLink_command_ack_message, [0, 1], 143 ),
-        MAVLINK_MSG_ID_HIL_STATE : ( '<Qffffffiiihhhhhh', MAVLink_hil_state_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15], 183 ),
-        MAVLINK_MSG_ID_HIL_CONTROLS : ( '<QffffffffBB', MAVLink_hil_controls_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10], 63 ),
-        MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW : ( '<QHHHHHHHHHHHHB', MAVLink_hil_rc_inputs_raw_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13], 54 ),
-        MAVLINK_MSG_ID_OPTICAL_FLOW : ( '<QfhhBB', MAVLink_optical_flow_message, [0, 4, 2, 3, 5, 1], 19 ),
-        MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE : ( '<Qffffff', MAVLink_global_vision_position_estimate_message, [0, 1, 2, 3, 4, 5, 6], 102 ),
-        MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE : ( '<Qffffff', MAVLink_vision_position_estimate_message, [0, 1, 2, 3, 4, 5, 6], 158 ),
-        MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE : ( '<Qfff', MAVLink_vision_speed_estimate_message, [0, 1, 2, 3], 208 ),
-        MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE : ( '<Qffffff', MAVLink_vicon_position_estimate_message, [0, 1, 2, 3, 4, 5, 6], 56 ),
-        MAVLINK_MSG_ID_MEMORY_VECT : ( '<HBB32s', MAVLink_memory_vect_message, [0, 1, 2, 3], 204 ),
-        MAVLINK_MSG_ID_DEBUG_VECT : ( '<Qfff10s', MAVLink_debug_vect_message, [4, 0, 1, 2, 3], 49 ),
-        MAVLINK_MSG_ID_NAMED_VALUE_FLOAT : ( '<If10s', MAVLink_named_value_float_message, [0, 2, 1], 170 ),
-        MAVLINK_MSG_ID_NAMED_VALUE_INT : ( '<Ii10s', MAVLink_named_value_int_message, [0, 2, 1], 44 ),
-        MAVLINK_MSG_ID_STATUSTEXT : ( '<B50s', MAVLink_statustext_message, [0, 1], 83 ),
-        MAVLINK_MSG_ID_DEBUG : ( '<IfB', MAVLink_debug_message, [0, 2, 1], 46 ),
-        MAVLINK_MSG_ID_EXTENDED_MESSAGE : ( '<BBB', MAVLink_extended_message_message, [0, 1, 2], 247 ),
-}
-
-class MAVError(Exception):
-        '''MAVLink error class'''
-        def __init__(self, msg):
-            Exception.__init__(self, msg)
-            self.message = msg
-
-class MAVString(str):
-        '''NUL terminated string'''
-        def __init__(self, s):
-                str.__init__(self)
-        def __str__(self):
-            i = self.find(chr(0))
-            if i == -1:
-                return self[:]
-            return self[0:i]
-
-class MAVLink_bad_data(MAVLink_message):
-        '''
-        a piece of bad data in a mavlink stream
-        '''
-        def __init__(self, data, reason):
-                MAVLink_message.__init__(self, MAVLINK_MSG_ID_BAD_DATA, 'BAD_DATA')
-                self._fieldnames = ['data', 'reason']
-                self.data = data
-                self.reason = reason
-                self._msgbuf = data
-            
-class MAVLink(object):
-        '''MAVLink protocol handling class'''
-        def __init__(self, file, srcSystem=0, srcComponent=0):
-                self.seq = 0
-                self.file = file
-                self.srcSystem = srcSystem
-                self.srcComponent = srcComponent
-                self.callback = None
-                self.callback_args = None
-                self.callback_kwargs = None
-                self.buf = array.array('B')
-                self.expected_length = 6
-                self.have_prefix_error = False
-                self.robust_parsing = False
-                self.protocol_marker = 254
-                self.little_endian = True
-                self.crc_extra = True
-                self.sort_fields = True
-                self.total_packets_sent = 0
-                self.total_bytes_sent = 0
-                self.total_packets_received = 0
-                self.total_bytes_received = 0
-                self.total_receive_errors = 0
-                self.startup_time = time.time()
-
-        def set_callback(self, callback, *args, **kwargs):
-            self.callback = callback
-            self.callback_args = args
-            self.callback_kwargs = kwargs
-            
-        def send(self, mavmsg):
-                '''send a MAVLink message'''
-                buf = mavmsg.pack(self)
-                self.file.write(buf)
-                self.seq = (self.seq + 1) % 255
-                self.total_packets_sent += 1
-                self.total_bytes_sent += len(buf)
-
-        def bytes_needed(self):
-            '''return number of bytes needed for next parsing stage'''
-            ret = self.expected_length - len(self.buf)
-            if ret <= 0:
-                return 1
-            return ret
-
-        def parse_char(self, c):
-            '''input some data bytes, possibly returning a new message'''
-            if isinstance(c, str):
-                self.buf.fromstring(c)
-            else:
-                self.buf.extend(c)
-            self.total_bytes_received += len(c)
-            if len(self.buf) >= 1 and self.buf[0] != 254:
-                magic = self.buf[0]
-                self.buf = self.buf[1:]
-                if self.robust_parsing:
-                    m = MAVLink_bad_data(chr(magic), "Bad prefix")
-                    if self.callback:
-                        self.callback(m, *self.callback_args, **self.callback_kwargs)
-                    self.expected_length = 6
-                    self.total_receive_errors += 1
-                    return m
-                if self.have_prefix_error:
-                    return None
-                self.have_prefix_error = True
-                self.total_receive_errors += 1
-                raise MAVError("invalid MAVLink prefix '%s'" % magic) 
-            self.have_prefix_error = False
-            if len(self.buf) >= 2:
-                (magic, self.expected_length) = struct.unpack('BB', self.buf[0:2])
-                self.expected_length += 8
-            if self.expected_length >= 8 and len(self.buf) >= self.expected_length:
-                mbuf = self.buf[0:self.expected_length]
-                self.buf = self.buf[self.expected_length:]
-                self.expected_length = 6
-                if self.robust_parsing:
-                    try:
-                        m = self.decode(mbuf)
-                        self.total_packets_received += 1
-                    except MAVError as reason:
-                        m = MAVLink_bad_data(mbuf, reason.message)
-                        self.total_receive_errors += 1
-                else:
-                    m = self.decode(mbuf)
-                    self.total_packets_received += 1
-                if self.callback:
-                    self.callback(m, *self.callback_args, **self.callback_kwargs)
-                return m
-            return None
-
-        def parse_buffer(self, s):
-            '''input some data bytes, possibly returning a list of new messages'''
-            m = self.parse_char(s)
-            if m is None:
-                return None
-            ret = [m]
-            while True:
-                m = self.parse_char("")
-                if m is None:
-                    return ret
-                ret.append(m)
-            return ret
-
-        def decode(self, msgbuf):
-                '''decode a buffer as a MAVLink message'''
-                # decode the header
-                try:
-                    magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
-                if ord(magic) != 254:
-                    raise MAVError("invalid MAVLink prefix '%s'" % magic)
-                if mlen != len(msgbuf)-8:
-                    raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf)-8, mlen, msgId))
-
-                if not msgId in mavlink_map:
-                    raise MAVError('unknown MAVLink message ID %u' % msgId)
-
-                # decode the payload
-                (fmt, type, order_map, crc_extra) = mavlink_map[msgId]
-
-                # decode the checksum
-                try:
-                    crc, = struct.unpack('<H', msgbuf[-2:])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink CRC: %s' % emsg)
-                crc2 = mavutil.x25crc(msgbuf[1:-2])
-                if True: # using CRC extra 
-                    crc2.accumulate(chr(crc_extra))
-                if crc != crc2.crc:
-                    raise MAVError('invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' % (msgId, crc, crc2.crc))
-
-                try:
-                    t = struct.unpack(fmt, msgbuf[6:-2])
-                except struct.error as emsg:
-                    raise MAVError('Unable to unpack MAVLink payload type=%s fmt=%s payloadLength=%u: %s' % (
-                        type, fmt, len(msgbuf[6:-2]), emsg))
-
-                tlist = list(t)
-                # handle sorted fields
-                if True:
-                    t = tlist[:]
-                    for i in range(0, len(tlist)):
-                        tlist[i] = t[order_map[i]]
-
-                # terminate any strings
-                for i in range(0, len(tlist)):
-                    if isinstance(tlist[i], str):
-                        tlist[i] = MAVString(tlist[i])
-                t = tuple(tlist)
-                # construct the message object
-                try:
-                    m = type(*t)
-                except Exception as emsg:
-                    raise MAVError('Unable to instantiate MAVLink message of type %s : %s' % (type, emsg))
-                m._msgbuf = msgbuf
-                m._payload = msgbuf[6:-2]
-                m._crc = crc
-                m._header = MAVLink_header(msgId, mlen, seq, srcSystem, srcComponent)
-                return m
-        def sensor_offsets_encode(self, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z):
-                '''
-                Offsets and calibrations values for hardware         sensors. This
-                makes it easier to debug the calibration process.
-
-                mag_ofs_x                 : magnetometer X offset (int16_t)
-                mag_ofs_y                 : magnetometer Y offset (int16_t)
-                mag_ofs_z                 : magnetometer Z offset (int16_t)
-                mag_declination           : magnetic declination (radians) (float)
-                raw_press                 : raw pressure from barometer (int32_t)
-                raw_temp                  : raw temperature from barometer (int32_t)
-                gyro_cal_x                : gyro X calibration (float)
-                gyro_cal_y                : gyro Y calibration (float)
-                gyro_cal_z                : gyro Z calibration (float)
-                accel_cal_x               : accel X calibration (float)
-                accel_cal_y               : accel Y calibration (float)
-                accel_cal_z               : accel Z calibration (float)
-
-                '''
-                msg = MAVLink_sensor_offsets_message(mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z)
-                msg.pack(self)
-                return msg
-            
-        def sensor_offsets_send(self, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z):
-                '''
-                Offsets and calibrations values for hardware         sensors. This
-                makes it easier to debug the calibration process.
-
-                mag_ofs_x                 : magnetometer X offset (int16_t)
-                mag_ofs_y                 : magnetometer Y offset (int16_t)
-                mag_ofs_z                 : magnetometer Z offset (int16_t)
-                mag_declination           : magnetic declination (radians) (float)
-                raw_press                 : raw pressure from barometer (int32_t)
-                raw_temp                  : raw temperature from barometer (int32_t)
-                gyro_cal_x                : gyro X calibration (float)
-                gyro_cal_y                : gyro Y calibration (float)
-                gyro_cal_z                : gyro Z calibration (float)
-                accel_cal_x               : accel X calibration (float)
-                accel_cal_y               : accel Y calibration (float)
-                accel_cal_z               : accel Z calibration (float)
-
-                '''
-                return self.send(self.sensor_offsets_encode(mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z))
-            
-        def set_mag_offsets_encode(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z):
-                '''
-                set the magnetometer offsets
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mag_ofs_x                 : magnetometer X offset (int16_t)
-                mag_ofs_y                 : magnetometer Y offset (int16_t)
-                mag_ofs_z                 : magnetometer Z offset (int16_t)
-
-                '''
-                msg = MAVLink_set_mag_offsets_message(target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z)
-                msg.pack(self)
-                return msg
-            
-        def set_mag_offsets_send(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z):
-                '''
-                set the magnetometer offsets
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mag_ofs_x                 : magnetometer X offset (int16_t)
-                mag_ofs_y                 : magnetometer Y offset (int16_t)
-                mag_ofs_z                 : magnetometer Z offset (int16_t)
-
-                '''
-                return self.send(self.set_mag_offsets_encode(target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z))
-            
-        def meminfo_encode(self, brkval, freemem):
-                '''
-                state of APM memory
-
-                brkval                    : heap top (uint16_t)
-                freemem                   : free memory (uint16_t)
-
-                '''
-                msg = MAVLink_meminfo_message(brkval, freemem)
-                msg.pack(self)
-                return msg
-            
-        def meminfo_send(self, brkval, freemem):
-                '''
-                state of APM memory
-
-                brkval                    : heap top (uint16_t)
-                freemem                   : free memory (uint16_t)
-
-                '''
-                return self.send(self.meminfo_encode(brkval, freemem))
-            
-        def ap_adc_encode(self, adc1, adc2, adc3, adc4, adc5, adc6):
-                '''
-                raw ADC output
-
-                adc1                      : ADC output 1 (uint16_t)
-                adc2                      : ADC output 2 (uint16_t)
-                adc3                      : ADC output 3 (uint16_t)
-                adc4                      : ADC output 4 (uint16_t)
-                adc5                      : ADC output 5 (uint16_t)
-                adc6                      : ADC output 6 (uint16_t)
-
-                '''
-                msg = MAVLink_ap_adc_message(adc1, adc2, adc3, adc4, adc5, adc6)
-                msg.pack(self)
-                return msg
-            
-        def ap_adc_send(self, adc1, adc2, adc3, adc4, adc5, adc6):
-                '''
-                raw ADC output
-
-                adc1                      : ADC output 1 (uint16_t)
-                adc2                      : ADC output 2 (uint16_t)
-                adc3                      : ADC output 3 (uint16_t)
-                adc4                      : ADC output 4 (uint16_t)
-                adc5                      : ADC output 5 (uint16_t)
-                adc6                      : ADC output 6 (uint16_t)
-
-                '''
-                return self.send(self.ap_adc_encode(adc1, adc2, adc3, adc4, adc5, adc6))
-            
-        def digicam_configure_encode(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value):
-                '''
-                Configure on-board Camera Control System.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mode                      : Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) (uint8_t)
-                shutter_speed             : Divisor number //e.g. 1000 means 1/1000 (0 means ignore) (uint16_t)
-                aperture                  : F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) (uint8_t)
-                iso                       : ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) (uint8_t)
-                exposure_type             : Exposure type enumeration from 1 to N (0 means ignore) (uint8_t)
-                command_id                : Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once (uint8_t)
-                engine_cut_off            : Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) (uint8_t)
-                extra_param               : Extra parameters enumeration (0 means ignore) (uint8_t)
-                extra_value               : Correspondent value to given extra_param (float)
-
-                '''
-                msg = MAVLink_digicam_configure_message(target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value)
-                msg.pack(self)
-                return msg
-            
-        def digicam_configure_send(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value):
-                '''
-                Configure on-board Camera Control System.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mode                      : Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) (uint8_t)
-                shutter_speed             : Divisor number //e.g. 1000 means 1/1000 (0 means ignore) (uint16_t)
-                aperture                  : F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) (uint8_t)
-                iso                       : ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) (uint8_t)
-                exposure_type             : Exposure type enumeration from 1 to N (0 means ignore) (uint8_t)
-                command_id                : Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once (uint8_t)
-                engine_cut_off            : Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) (uint8_t)
-                extra_param               : Extra parameters enumeration (0 means ignore) (uint8_t)
-                extra_value               : Correspondent value to given extra_param (float)
-
-                '''
-                return self.send(self.digicam_configure_encode(target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value))
-            
-        def digicam_control_encode(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value):
-                '''
-                Control on-board Camera Control System to take shots.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                session                   : 0: stop, 1: start or keep it up //Session control e.g. show/hide lens (uint8_t)
-                zoom_pos                  : 1 to N //Zoom's absolute position (0 means ignore) (uint8_t)
-                zoom_step                 : -100 to 100 //Zooming step value to offset zoom from the current position (int8_t)
-                focus_lock                : 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus (uint8_t)
-                shot                      : 0: ignore, 1: shot or start filming (uint8_t)
-                command_id                : Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once (uint8_t)
-                extra_param               : Extra parameters enumeration (0 means ignore) (uint8_t)
-                extra_value               : Correspondent value to given extra_param (float)
-
-                '''
-                msg = MAVLink_digicam_control_message(target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value)
-                msg.pack(self)
-                return msg
-            
-        def digicam_control_send(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value):
-                '''
-                Control on-board Camera Control System to take shots.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                session                   : 0: stop, 1: start or keep it up //Session control e.g. show/hide lens (uint8_t)
-                zoom_pos                  : 1 to N //Zoom's absolute position (0 means ignore) (uint8_t)
-                zoom_step                 : -100 to 100 //Zooming step value to offset zoom from the current position (int8_t)
-                focus_lock                : 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus (uint8_t)
-                shot                      : 0: ignore, 1: shot or start filming (uint8_t)
-                command_id                : Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once (uint8_t)
-                extra_param               : Extra parameters enumeration (0 means ignore) (uint8_t)
-                extra_value               : Correspondent value to given extra_param (float)
-
-                '''
-                return self.send(self.digicam_control_encode(target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value))
-            
-        def mount_configure_encode(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw):
-                '''
-                Message to configure a camera mount, directional antenna, etc.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mount_mode                : mount operating mode (see MAV_MOUNT_MODE enum) (uint8_t)
-                stab_roll                 : (1 = yes, 0 = no) (uint8_t)
-                stab_pitch                : (1 = yes, 0 = no) (uint8_t)
-                stab_yaw                  : (1 = yes, 0 = no) (uint8_t)
-
-                '''
-                msg = MAVLink_mount_configure_message(target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw)
-                msg.pack(self)
-                return msg
-            
-        def mount_configure_send(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw):
-                '''
-                Message to configure a camera mount, directional antenna, etc.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                mount_mode                : mount operating mode (see MAV_MOUNT_MODE enum) (uint8_t)
-                stab_roll                 : (1 = yes, 0 = no) (uint8_t)
-                stab_pitch                : (1 = yes, 0 = no) (uint8_t)
-                stab_yaw                  : (1 = yes, 0 = no) (uint8_t)
-
-                '''
-                return self.send(self.mount_configure_encode(target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw))
-            
-        def mount_control_encode(self, target_system, target_component, input_a, input_b, input_c, save_position):
-                '''
-                Message to control a camera mount, directional antenna, etc.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                input_a                   : pitch(deg*100) or lat, depending on mount mode (int32_t)
-                input_b                   : roll(deg*100) or lon depending on mount mode (int32_t)
-                input_c                   : yaw(deg*100) or alt (in cm) depending on mount mode (int32_t)
-                save_position             : if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) (uint8_t)
-
-                '''
-                msg = MAVLink_mount_control_message(target_system, target_component, input_a, input_b, input_c, save_position)
-                msg.pack(self)
-                return msg
-            
-        def mount_control_send(self, target_system, target_component, input_a, input_b, input_c, save_position):
-                '''
-                Message to control a camera mount, directional antenna, etc.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                input_a                   : pitch(deg*100) or lat, depending on mount mode (int32_t)
-                input_b                   : roll(deg*100) or lon depending on mount mode (int32_t)
-                input_c                   : yaw(deg*100) or alt (in cm) depending on mount mode (int32_t)
-                save_position             : if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) (uint8_t)
-
-                '''
-                return self.send(self.mount_control_encode(target_system, target_component, input_a, input_b, input_c, save_position))
-            
-        def mount_status_encode(self, target_system, target_component, pointing_a, pointing_b, pointing_c):
-                '''
-                Message with some status from APM to GCS about camera or antenna mount
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                pointing_a                : pitch(deg*100) or lat, depending on mount mode (int32_t)
-                pointing_b                : roll(deg*100) or lon depending on mount mode (int32_t)
-                pointing_c                : yaw(deg*100) or alt (in cm) depending on mount mode (int32_t)
-
-                '''
-                msg = MAVLink_mount_status_message(target_system, target_component, pointing_a, pointing_b, pointing_c)
-                msg.pack(self)
-                return msg
-            
-        def mount_status_send(self, target_system, target_component, pointing_a, pointing_b, pointing_c):
-                '''
-                Message with some status from APM to GCS about camera or antenna mount
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                pointing_a                : pitch(deg*100) or lat, depending on mount mode (int32_t)
-                pointing_b                : roll(deg*100) or lon depending on mount mode (int32_t)
-                pointing_c                : yaw(deg*100) or alt (in cm) depending on mount mode (int32_t)
-
-                '''
-                return self.send(self.mount_status_encode(target_system, target_component, pointing_a, pointing_b, pointing_c))
-            
-        def fence_point_encode(self, target_system, target_component, idx, count, lat, lng):
-                '''
-                A fence point. Used to set a point when from               GCS -> MAV.
-                Also used to return a point from MAV -> GCS
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
-                count                     : total number of points (for sanity checking) (uint8_t)
-                lat                       : Latitude of point (float)
-                lng                       : Longitude of point (float)
-
-                '''
-                msg = MAVLink_fence_point_message(target_system, target_component, idx, count, lat, lng)
-                msg.pack(self)
-                return msg
-            
-        def fence_point_send(self, target_system, target_component, idx, count, lat, lng):
-                '''
-                A fence point. Used to set a point when from               GCS -> MAV.
-                Also used to return a point from MAV -> GCS
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
-                count                     : total number of points (for sanity checking) (uint8_t)
-                lat                       : Latitude of point (float)
-                lng                       : Longitude of point (float)
-
-                '''
-                return self.send(self.fence_point_encode(target_system, target_component, idx, count, lat, lng))
-            
-        def fence_fetch_point_encode(self, target_system, target_component, idx):
-                '''
-                Request a current fence point from MAV
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
-
-                '''
-                msg = MAVLink_fence_fetch_point_message(target_system, target_component, idx)
-                msg.pack(self)
-                return msg
-            
-        def fence_fetch_point_send(self, target_system, target_component, idx):
-                '''
-                Request a current fence point from MAV
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
-
-                '''
-                return self.send(self.fence_fetch_point_encode(target_system, target_component, idx))
-            
-        def fence_status_encode(self, breach_status, breach_count, breach_type, breach_time):
-                '''
-                Status of geo-fencing. Sent in extended             status stream when
-                fencing enabled
-
-                breach_status             : 0 if currently inside fence, 1 if outside (uint8_t)
-                breach_count              : number of fence breaches (uint16_t)
-                breach_type               : last breach type (see FENCE_BREACH_* enum) (uint8_t)
-                breach_time               : time of last breach in milliseconds since boot (uint32_t)
-
-                '''
-                msg = MAVLink_fence_status_message(breach_status, breach_count, breach_type, breach_time)
-                msg.pack(self)
-                return msg
-            
-        def fence_status_send(self, breach_status, breach_count, breach_type, breach_time):
-                '''
-                Status of geo-fencing. Sent in extended             status stream when
-                fencing enabled
-
-                breach_status             : 0 if currently inside fence, 1 if outside (uint8_t)
-                breach_count              : number of fence breaches (uint16_t)
-                breach_type               : last breach type (see FENCE_BREACH_* enum) (uint8_t)
-                breach_time               : time of last breach in milliseconds since boot (uint32_t)
-
-                '''
-                return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time))
-            
-        def ahrs_encode(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw):
-                '''
-                Status of DCM attitude estimator
-
-                omegaIx                   : X gyro drift estimate rad/s (float)
-                omegaIy                   : Y gyro drift estimate rad/s (float)
-                omegaIz                   : Z gyro drift estimate rad/s (float)
-                accel_weight              : average accel_weight (float)
-                renorm_val                : average renormalisation value (float)
-                error_rp                  : average error_roll_pitch value (float)
-                error_yaw                 : average error_yaw value (float)
-
-                '''
-                msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)
-                msg.pack(self)
-                return msg
-            
-        def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw):
-                '''
-                Status of DCM attitude estimator
-
-                omegaIx                   : X gyro drift estimate rad/s (float)
-                omegaIy                   : Y gyro drift estimate rad/s (float)
-                omegaIz                   : Z gyro drift estimate rad/s (float)
-                accel_weight              : average accel_weight (float)
-                renorm_val                : average renormalisation value (float)
-                error_rp                  : average error_roll_pitch value (float)
-                error_yaw                 : average error_yaw value (float)
-
-                '''
-                return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw))
-            
-        def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro):
-                '''
-                Status of simulation environment, if used
-
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                xacc                      : X acceleration m/s/s (float)
-                yacc                      : Y acceleration m/s/s (float)
-                zacc                      : Z acceleration m/s/s (float)
-                xgyro                     : Angular speed around X axis rad/s (float)
-                ygyro                     : Angular speed around Y axis rad/s (float)
-                zgyro                     : Angular speed around Z axis rad/s (float)
-
-                '''
-                msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)
-                msg.pack(self)
-                return msg
-            
-        def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro):
-                '''
-                Status of simulation environment, if used
-
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                xacc                      : X acceleration m/s/s (float)
-                yacc                      : Y acceleration m/s/s (float)
-                zacc                      : Z acceleration m/s/s (float)
-                xgyro                     : Angular speed around X axis rad/s (float)
-                ygyro                     : Angular speed around Y axis rad/s (float)
-                zgyro                     : Angular speed around Z axis rad/s (float)
-
-                '''
-                return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro))
-            
-        def hwstatus_encode(self, Vcc, I2Cerr):
-                '''
-                Status of key hardware
-
-                Vcc                       : board voltage (mV) (uint16_t)
-                I2Cerr                    : I2C error count (uint8_t)
-
-                '''
-                msg = MAVLink_hwstatus_message(Vcc, I2Cerr)
-                msg.pack(self)
-                return msg
-            
-        def hwstatus_send(self, Vcc, I2Cerr):
-                '''
-                Status of key hardware
-
-                Vcc                       : board voltage (mV) (uint16_t)
-                I2Cerr                    : I2C error count (uint8_t)
-
-                '''
-                return self.send(self.hwstatus_encode(Vcc, I2Cerr))
-            
-        def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed):
-                '''
-                Status generated by radio
-
-                rssi                      : local signal strength (uint8_t)
-                remrssi                   : remote signal strength (uint8_t)
-                txbuf                     : how full the tx buffer is as a percentage (uint8_t)
-                noise                     : background noise level (uint8_t)
-                remnoise                  : remote background noise level (uint8_t)
-                rxerrors                  : receive errors (uint16_t)
-                fixed                     : count of error corrected packets (uint16_t)
-
-                '''
-                msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)
-                msg.pack(self)
-                return msg
-            
-        def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed):
-                '''
-                Status generated by radio
-
-                rssi                      : local signal strength (uint8_t)
-                remrssi                   : remote signal strength (uint8_t)
-                txbuf                     : how full the tx buffer is as a percentage (uint8_t)
-                noise                     : background noise level (uint8_t)
-                remnoise                  : remote background noise level (uint8_t)
-                rxerrors                  : receive errors (uint16_t)
-                fixed                     : count of error corrected packets (uint16_t)
-
-                '''
-                return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed))
-            
-        def heartbeat_encode(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version=3):
-                '''
-                The heartbeat message shows that a system is present and responding.
-                The type of the MAV and Autopilot hardware allow the
-                receiving system to treat further messages from this
-                system appropriate (e.g. by laying out the user
-                interface based on the autopilot).
-
-                type                      : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t)
-                autopilot                 : Autopilot type / class. defined in MAV_CLASS ENUM (uint8_t)
-                base_mode                 : System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h (uint8_t)
-                custom_mode               : Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. (uint32_t)
-                system_status             : System status flag, see MAV_STATUS ENUM (uint8_t)
-                mavlink_version           : MAVLink version (uint8_t)
-
-                '''
-                msg = MAVLink_heartbeat_message(type, autopilot, base_mode, custom_mode, system_status, mavlink_version)
-                msg.pack(self)
-                return msg
-            
-        def heartbeat_send(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version=3):
-                '''
-                The heartbeat message shows that a system is present and responding.
-                The type of the MAV and Autopilot hardware allow the
-                receiving system to treat further messages from this
-                system appropriate (e.g. by laying out the user
-                interface based on the autopilot).
-
-                type                      : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t)
-                autopilot                 : Autopilot type / class. defined in MAV_CLASS ENUM (uint8_t)
-                base_mode                 : System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h (uint8_t)
-                custom_mode               : Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. (uint32_t)
-                system_status             : System status flag, see MAV_STATUS ENUM (uint8_t)
-                mavlink_version           : MAVLink version (uint8_t)
-
-                '''
-                return self.send(self.heartbeat_encode(type, autopilot, base_mode, custom_mode, system_status, mavlink_version))
-            
-        def sys_status_encode(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4):
-                '''
-                The general system state. If the system is following the MAVLink
-                standard, the system state is mainly defined by three
-                orthogonal states/modes: The system mode, which is
-                either LOCKED (motors shut down and locked), MANUAL
-                (system under RC control), GUIDED (system with
-                autonomous position control, position setpoint
-                controlled manually) or AUTO (system guided by
-                path/waypoint planner). The NAV_MODE defined the
-                current flight state: LIFTOFF (often an open-loop
-                maneuver), LANDING, WAYPOINTS or VECTOR. This
-                represents the internal navigation state machine. The
-                system status shows wether the system is currently
-                active or not and if an emergency occured. During the
-                CRITICAL and EMERGENCY states the MAV is still
-                considered to be active, but should start emergency
-                procedures autonomously. After a failure occured it
-                should first move from active to critical to allow
-                manual intervention and then move to emergency after a
-                certain timeout.
-
-                onboard_control_sensors_present        : Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t)
-                onboard_control_sensors_enabled        : Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t)
-                onboard_control_sensors_health        : Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t)
-                load                      : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t)
-                voltage_battery           : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t)
-                current_battery           : Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current (int16_t)
-                battery_remaining         : Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery (int8_t)
-                drop_rate_comm            : Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t)
-                errors_comm               : Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t)
-                errors_count1             : Autopilot-specific errors (uint16_t)
-                errors_count2             : Autopilot-specific errors (uint16_t)
-                errors_count3             : Autopilot-specific errors (uint16_t)
-                errors_count4             : Autopilot-specific errors (uint16_t)
-
-                '''
-                msg = MAVLink_sys_status_message(onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4)
-                msg.pack(self)
-                return msg
-            
-        def sys_status_send(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4):
-                '''
-                The general system state. If the system is following the MAVLink
-                standard, the system state is mainly defined by three
-                orthogonal states/modes: The system mode, which is
-                either LOCKED (motors shut down and locked), MANUAL
-                (system under RC control), GUIDED (system with
-                autonomous position control, position setpoint
-                controlled manually) or AUTO (system guided by
-                path/waypoint planner). The NAV_MODE defined the
-                current flight state: LIFTOFF (often an open-loop
-                maneuver), LANDING, WAYPOINTS or VECTOR. This
-                represents the internal navigation state machine. The
-                system status shows wether the system is currently
-                active or not and if an emergency occured. During the
-                CRITICAL and EMERGENCY states the MAV is still
-                considered to be active, but should start emergency
-                procedures autonomously. After a failure occured it
-                should first move from active to critical to allow
-                manual intervention and then move to emergency after a
-                certain timeout.
-
-                onboard_control_sensors_present        : Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t)
-                onboard_control_sensors_enabled        : Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t)
-                onboard_control_sensors_health        : Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t)
-                load                      : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t)
-                voltage_battery           : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t)
-                current_battery           : Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current (int16_t)
-                battery_remaining         : Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery (int8_t)
-                drop_rate_comm            : Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t)
-                errors_comm               : Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t)
-                errors_count1             : Autopilot-specific errors (uint16_t)
-                errors_count2             : Autopilot-specific errors (uint16_t)
-                errors_count3             : Autopilot-specific errors (uint16_t)
-                errors_count4             : Autopilot-specific errors (uint16_t)
-
-                '''
-                return self.send(self.sys_status_encode(onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4))
-            
-        def system_time_encode(self, time_unix_usec, time_boot_ms):
-                '''
-                The system time is the time of the master clock, typically the
-                computer clock of the main onboard computer.
-
-                time_unix_usec            : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t)
-                time_boot_ms              : Timestamp of the component clock since boot time in milliseconds. (uint32_t)
-
-                '''
-                msg = MAVLink_system_time_message(time_unix_usec, time_boot_ms)
-                msg.pack(self)
-                return msg
-            
-        def system_time_send(self, time_unix_usec, time_boot_ms):
-                '''
-                The system time is the time of the master clock, typically the
-                computer clock of the main onboard computer.
-
-                time_unix_usec            : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t)
-                time_boot_ms              : Timestamp of the component clock since boot time in milliseconds. (uint32_t)
-
-                '''
-                return self.send(self.system_time_encode(time_unix_usec, time_boot_ms))
-            
-        def ping_encode(self, time_usec, seq, target_system, target_component):
-                '''
-                A ping message either requesting or responding to a ping. This allows
-                to measure the system latencies, including serial
-                port, radio modem and UDP connections.
-
-                time_usec                 : Unix timestamp in microseconds (uint64_t)
-                seq                       : PING sequence (uint32_t)
-                target_system             : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t)
-                target_component          : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t)
-
-                '''
-                msg = MAVLink_ping_message(time_usec, seq, target_system, target_component)
-                msg.pack(self)
-                return msg
-            
-        def ping_send(self, time_usec, seq, target_system, target_component):
-                '''
-                A ping message either requesting or responding to a ping. This allows
-                to measure the system latencies, including serial
-                port, radio modem and UDP connections.
-
-                time_usec                 : Unix timestamp in microseconds (uint64_t)
-                seq                       : PING sequence (uint32_t)
-                target_system             : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t)
-                target_component          : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t)
-
-                '''
-                return self.send(self.ping_encode(time_usec, seq, target_system, target_component))
-            
-        def change_operator_control_encode(self, target_system, control_request, version, passkey):
-                '''
-                Request to control this MAV
-
-                target_system             : System the GCS requests control for (uint8_t)
-                control_request           : 0: request control of this MAV, 1: Release control of this MAV (uint8_t)
-                version                   : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t)
-                passkey                   : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char)
-
-                '''
-                msg = MAVLink_change_operator_control_message(target_system, control_request, version, passkey)
-                msg.pack(self)
-                return msg
-            
-        def change_operator_control_send(self, target_system, control_request, version, passkey):
-                '''
-                Request to control this MAV
-
-                target_system             : System the GCS requests control for (uint8_t)
-                control_request           : 0: request control of this MAV, 1: Release control of this MAV (uint8_t)
-                version                   : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t)
-                passkey                   : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char)
-
-                '''
-                return self.send(self.change_operator_control_encode(target_system, control_request, version, passkey))
-            
-        def change_operator_control_ack_encode(self, gcs_system_id, control_request, ack):
-                '''
-                Accept / deny control of this MAV
-
-                gcs_system_id             : ID of the GCS this message (uint8_t)
-                control_request           : 0: request control of this MAV, 1: Release control of this MAV (uint8_t)
-                ack                       : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t)
-
-                '''
-                msg = MAVLink_change_operator_control_ack_message(gcs_system_id, control_request, ack)
-                msg.pack(self)
-                return msg
-            
-        def change_operator_control_ack_send(self, gcs_system_id, control_request, ack):
-                '''
-                Accept / deny control of this MAV
-
-                gcs_system_id             : ID of the GCS this message (uint8_t)
-                control_request           : 0: request control of this MAV, 1: Release control of this MAV (uint8_t)
-                ack                       : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t)
-
-                '''
-                return self.send(self.change_operator_control_ack_encode(gcs_system_id, control_request, ack))
-            
-        def auth_key_encode(self, key):
-                '''
-                Emit an encrypted signature / key identifying this system. PLEASE
-                NOTE: This protocol has been kept simple, so
-                transmitting the key requires an encrypted channel for
-                true safety.
-
-                key                       : key (char)
-
-                '''
-                msg = MAVLink_auth_key_message(key)
-                msg.pack(self)
-                return msg
-            
-        def auth_key_send(self, key):
-                '''
-                Emit an encrypted signature / key identifying this system. PLEASE
-                NOTE: This protocol has been kept simple, so
-                transmitting the key requires an encrypted channel for
-                true safety.
-
-                key                       : key (char)
-
-                '''
-                return self.send(self.auth_key_encode(key))
-            
-        def set_mode_encode(self, target_system, base_mode, custom_mode):
-                '''
-                Set the system mode, as defined by enum MAV_MODE. There is no target
-                component id as the mode is by definition for the
-                overall aircraft, not only for one component.
-
-                target_system             : The system setting the mode (uint8_t)
-                base_mode                 : The new base mode (uint8_t)
-                custom_mode               : The new autopilot-specific mode. This field can be ignored by an autopilot. (uint32_t)
-
-                '''
-                msg = MAVLink_set_mode_message(target_system, base_mode, custom_mode)
-                msg.pack(self)
-                return msg
-            
-        def set_mode_send(self, target_system, base_mode, custom_mode):
-                '''
-                Set the system mode, as defined by enum MAV_MODE. There is no target
-                component id as the mode is by definition for the
-                overall aircraft, not only for one component.
-
-                target_system             : The system setting the mode (uint8_t)
-                base_mode                 : The new base mode (uint8_t)
-                custom_mode               : The new autopilot-specific mode. This field can be ignored by an autopilot. (uint32_t)
-
-                '''
-                return self.send(self.set_mode_encode(target_system, base_mode, custom_mode))
-            
-        def param_request_read_encode(self, target_system, target_component, param_id, param_index):
-                '''
-                Request to read the onboard parameter with the param_id string id.
-                Onboard parameters are stored as key[const char*] ->
-                value[float]. This allows to send a parameter to any
-                other component (such as the GCS) without the need of
-                previous knowledge of possible parameter names. Thus
-                the same GCS can store different parameters for
-                different autopilots. See also
-                http://qgroundcontrol.org/parameter_interface for a
-                full documentation of QGroundControl and IMU code.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                param_id                  : Onboard parameter id (char)
-                param_index               : Parameter index. Send -1 to use the param ID field as identifier (int16_t)
-
-                '''
-                msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index)
-                msg.pack(self)
-                return msg
-            
-        def param_request_read_send(self, target_system, target_component, param_id, param_index):
-                '''
-                Request to read the onboard parameter with the param_id string id.
-                Onboard parameters are stored as key[const char*] ->
-                value[float]. This allows to send a parameter to any
-                other component (such as the GCS) without the need of
-                previous knowledge of possible parameter names. Thus
-                the same GCS can store different parameters for
-                different autopilots. See also
-                http://qgroundcontrol.org/parameter_interface for a
-                full documentation of QGroundControl and IMU code.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                param_id                  : Onboard parameter id (char)
-                param_index               : Parameter index. Send -1 to use the param ID field as identifier (int16_t)
-
-                '''
-                return self.send(self.param_request_read_encode(target_system, target_component, param_id, param_index))
-            
-        def param_request_list_encode(self, target_system, target_component):
-                '''
-                Request all parameters of this component. After his request, all
-                parameters are emitted.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                msg = MAVLink_param_request_list_message(target_system, target_component)
-                msg.pack(self)
-                return msg
-            
-        def param_request_list_send(self, target_system, target_component):
-                '''
-                Request all parameters of this component. After his request, all
-                parameters are emitted.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                return self.send(self.param_request_list_encode(target_system, target_component))
-            
-        def param_value_encode(self, param_id, param_value, param_type, param_count, param_index):
-                '''
-                Emit the value of a onboard parameter. The inclusion of param_count
-                and param_index in the message allows the recipient to
-                keep track of received parameters and allows him to
-                re-request missing parameters after a loss or timeout.
-
-                param_id                  : Onboard parameter id (char)
-                param_value               : Onboard parameter value (float)
-                param_type                : Onboard parameter type: see MAV_VAR enum (uint8_t)
-                param_count               : Total number of onboard parameters (uint16_t)
-                param_index               : Index of this onboard parameter (uint16_t)
-
-                '''
-                msg = MAVLink_param_value_message(param_id, param_value, param_type, param_count, param_index)
-                msg.pack(self)
-                return msg
-            
-        def param_value_send(self, param_id, param_value, param_type, param_count, param_index):
-                '''
-                Emit the value of a onboard parameter. The inclusion of param_count
-                and param_index in the message allows the recipient to
-                keep track of received parameters and allows him to
-                re-request missing parameters after a loss or timeout.
-
-                param_id                  : Onboard parameter id (char)
-                param_value               : Onboard parameter value (float)
-                param_type                : Onboard parameter type: see MAV_VAR enum (uint8_t)
-                param_count               : Total number of onboard parameters (uint16_t)
-                param_index               : Index of this onboard parameter (uint16_t)
-
-                '''
-                return self.send(self.param_value_encode(param_id, param_value, param_type, param_count, param_index))
-            
-        def param_set_encode(self, target_system, target_component, param_id, param_value, param_type):
-                '''
-                Set a parameter value TEMPORARILY to RAM. It will be reset to default
-                on system reboot. Send the ACTION
-                MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM
-                contents to EEPROM. IMPORTANT: The receiving component
-                should acknowledge the new parameter value by sending
-                a param_value message to all communication partners.
-                This will also ensure that multiple GCS all have an
-                up-to-date list of all parameters. If the sending GCS
-                did not receive a PARAM_VALUE message within its
-                timeout time, it should re-send the PARAM_SET message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                param_id                  : Onboard parameter id (char)
-                param_value               : Onboard parameter value (float)
-                param_type                : Onboard parameter type: see MAV_VAR enum (uint8_t)
-
-                '''
-                msg = MAVLink_param_set_message(target_system, target_component, param_id, param_value, param_type)
-                msg.pack(self)
-                return msg
-            
-        def param_set_send(self, target_system, target_component, param_id, param_value, param_type):
-                '''
-                Set a parameter value TEMPORARILY to RAM. It will be reset to default
-                on system reboot. Send the ACTION
-                MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM
-                contents to EEPROM. IMPORTANT: The receiving component
-                should acknowledge the new parameter value by sending
-                a param_value message to all communication partners.
-                This will also ensure that multiple GCS all have an
-                up-to-date list of all parameters. If the sending GCS
-                did not receive a PARAM_VALUE message within its
-                timeout time, it should re-send the PARAM_SET message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                param_id                  : Onboard parameter id (char)
-                param_value               : Onboard parameter value (float)
-                param_type                : Onboard parameter type: see MAV_VAR enum (uint8_t)
-
-                '''
-                return self.send(self.param_set_encode(target_system, target_component, param_id, param_value, param_type))
-            
-        def gps_raw_int_encode(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible):
-                '''
-                The global position, as returned by the Global Positioning System
-                (GPS). This is                 NOT the global position
-                estimate of the sytem, but rather a RAW sensor value.
-                See message GLOBAL_POSITION for the global position
-                estimate. Coordinate frame is right-handed, Z-axis up
-                (GPS frame)
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_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. (uint8_t)
-                lat                       : Latitude in 1E7 degrees (int32_t)
-                lon                       : Longitude in 1E7 degrees (int32_t)
-                alt                       : Altitude in 1E3 meters (millimeters) above MSL (int32_t)
-                eph                       : GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t)
-                epv                       : GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t)
-                vel                       : GPS ground speed (m/s * 100). If unknown, set to: 65535 (uint16_t)
-                cog                       : Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t)
-                satellites_visible        : Number of satellites visible. If unknown, set to 255 (uint8_t)
-
-                '''
-                msg = MAVLink_gps_raw_int_message(time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible)
-                msg.pack(self)
-                return msg
-            
-        def gps_raw_int_send(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible):
-                '''
-                The global position, as returned by the Global Positioning System
-                (GPS). This is                 NOT the global position
-                estimate of the sytem, but rather a RAW sensor value.
-                See message GLOBAL_POSITION for the global position
-                estimate. Coordinate frame is right-handed, Z-axis up
-                (GPS frame)
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_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. (uint8_t)
-                lat                       : Latitude in 1E7 degrees (int32_t)
-                lon                       : Longitude in 1E7 degrees (int32_t)
-                alt                       : Altitude in 1E3 meters (millimeters) above MSL (int32_t)
-                eph                       : GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t)
-                epv                       : GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t)
-                vel                       : GPS ground speed (m/s * 100). If unknown, set to: 65535 (uint16_t)
-                cog                       : Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t)
-                satellites_visible        : Number of satellites visible. If unknown, set to 255 (uint8_t)
-
-                '''
-                return self.send(self.gps_raw_int_encode(time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible))
-            
-        def gps_status_encode(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr):
-                '''
-                The positioning status, as reported by GPS. This message is intended
-                to display status information about each satellite
-                visible to the receiver. See message GLOBAL_POSITION
-                for the global position estimate. This message can
-                contain information for up to 20 satellites.
-
-                satellites_visible        : Number of satellites visible (uint8_t)
-                satellite_prn             : Global satellite ID (uint8_t)
-                satellite_used            : 0: Satellite not used, 1: used for localization (uint8_t)
-                satellite_elevation        : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (uint8_t)
-                satellite_azimuth         : Direction of satellite, 0: 0 deg, 255: 360 deg. (uint8_t)
-                satellite_snr             : Signal to noise ratio of satellite (uint8_t)
-
-                '''
-                msg = MAVLink_gps_status_message(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr)
-                msg.pack(self)
-                return msg
-            
-        def gps_status_send(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr):
-                '''
-                The positioning status, as reported by GPS. This message is intended
-                to display status information about each satellite
-                visible to the receiver. See message GLOBAL_POSITION
-                for the global position estimate. This message can
-                contain information for up to 20 satellites.
-
-                satellites_visible        : Number of satellites visible (uint8_t)
-                satellite_prn             : Global satellite ID (uint8_t)
-                satellite_used            : 0: Satellite not used, 1: used for localization (uint8_t)
-                satellite_elevation        : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (uint8_t)
-                satellite_azimuth         : Direction of satellite, 0: 0 deg, 255: 360 deg. (uint8_t)
-                satellite_snr             : Signal to noise ratio of satellite (uint8_t)
-
-                '''
-                return self.send(self.gps_status_encode(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr))
-            
-        def scaled_imu_encode(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                '''
-                The RAW IMU readings for the usual 9DOF sensor setup. This message
-                should contain the scaled values to the described
-                units
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                xacc                      : X acceleration (mg) (int16_t)
-                yacc                      : Y acceleration (mg) (int16_t)
-                zacc                      : Z acceleration (mg) (int16_t)
-                xgyro                     : Angular speed around X axis (millirad /sec) (int16_t)
-                ygyro                     : Angular speed around Y axis (millirad /sec) (int16_t)
-                zgyro                     : Angular speed around Z axis (millirad /sec) (int16_t)
-                xmag                      : X Magnetic field (milli tesla) (int16_t)
-                ymag                      : Y Magnetic field (milli tesla) (int16_t)
-                zmag                      : Z Magnetic field (milli tesla) (int16_t)
-
-                '''
-                msg = MAVLink_scaled_imu_message(time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)
-                msg.pack(self)
-                return msg
-            
-        def scaled_imu_send(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                '''
-                The RAW IMU readings for the usual 9DOF sensor setup. This message
-                should contain the scaled values to the described
-                units
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                xacc                      : X acceleration (mg) (int16_t)
-                yacc                      : Y acceleration (mg) (int16_t)
-                zacc                      : Z acceleration (mg) (int16_t)
-                xgyro                     : Angular speed around X axis (millirad /sec) (int16_t)
-                ygyro                     : Angular speed around Y axis (millirad /sec) (int16_t)
-                zgyro                     : Angular speed around Z axis (millirad /sec) (int16_t)
-                xmag                      : X Magnetic field (milli tesla) (int16_t)
-                ymag                      : Y Magnetic field (milli tesla) (int16_t)
-                zmag                      : Z Magnetic field (milli tesla) (int16_t)
-
-                '''
-                return self.send(self.scaled_imu_encode(time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag))
-            
-        def raw_imu_encode(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                '''
-                The RAW IMU readings for the usual 9DOF sensor setup. This message
-                should always contain the true raw values without any
-                scaling to allow data capture and system debugging.
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                xacc                      : X acceleration (raw) (int16_t)
-                yacc                      : Y acceleration (raw) (int16_t)
-                zacc                      : Z acceleration (raw) (int16_t)
-                xgyro                     : Angular speed around X axis (raw) (int16_t)
-                ygyro                     : Angular speed around Y axis (raw) (int16_t)
-                zgyro                     : Angular speed around Z axis (raw) (int16_t)
-                xmag                      : X Magnetic field (raw) (int16_t)
-                ymag                      : Y Magnetic field (raw) (int16_t)
-                zmag                      : Z Magnetic field (raw) (int16_t)
-
-                '''
-                msg = MAVLink_raw_imu_message(time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)
-                msg.pack(self)
-                return msg
-            
-        def raw_imu_send(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag):
-                '''
-                The RAW IMU readings for the usual 9DOF sensor setup. This message
-                should always contain the true raw values without any
-                scaling to allow data capture and system debugging.
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                xacc                      : X acceleration (raw) (int16_t)
-                yacc                      : Y acceleration (raw) (int16_t)
-                zacc                      : Z acceleration (raw) (int16_t)
-                xgyro                     : Angular speed around X axis (raw) (int16_t)
-                ygyro                     : Angular speed around Y axis (raw) (int16_t)
-                zgyro                     : Angular speed around Z axis (raw) (int16_t)
-                xmag                      : X Magnetic field (raw) (int16_t)
-                ymag                      : Y Magnetic field (raw) (int16_t)
-                zmag                      : Z Magnetic field (raw) (int16_t)
-
-                '''
-                return self.send(self.raw_imu_encode(time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag))
-            
-        def raw_pressure_encode(self, time_usec, press_abs, press_diff1, press_diff2, temperature):
-                '''
-                The RAW pressure readings for the typical setup of one absolute
-                pressure and one differential pressure sensor. The
-                sensor values should be the raw, UNSCALED ADC values.
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                press_abs                 : Absolute pressure (raw) (int16_t)
-                press_diff1               : Differential pressure 1 (raw) (int16_t)
-                press_diff2               : Differential pressure 2 (raw) (int16_t)
-                temperature               : Raw Temperature measurement (raw) (int16_t)
-
-                '''
-                msg = MAVLink_raw_pressure_message(time_usec, press_abs, press_diff1, press_diff2, temperature)
-                msg.pack(self)
-                return msg
-            
-        def raw_pressure_send(self, time_usec, press_abs, press_diff1, press_diff2, temperature):
-                '''
-                The RAW pressure readings for the typical setup of one absolute
-                pressure and one differential pressure sensor. The
-                sensor values should be the raw, UNSCALED ADC values.
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                press_abs                 : Absolute pressure (raw) (int16_t)
-                press_diff1               : Differential pressure 1 (raw) (int16_t)
-                press_diff2               : Differential pressure 2 (raw) (int16_t)
-                temperature               : Raw Temperature measurement (raw) (int16_t)
-
-                '''
-                return self.send(self.raw_pressure_encode(time_usec, press_abs, press_diff1, press_diff2, temperature))
-            
-        def scaled_pressure_encode(self, time_boot_ms, press_abs, press_diff, temperature):
-                '''
-                The pressure readings for the typical setup of one absolute and
-                differential pressure sensor. The units are as
-                specified in each field.
-
-                time_boot_ms              : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint32_t)
-                press_abs                 : Absolute pressure (hectopascal) (float)
-                press_diff                : Differential pressure 1 (hectopascal) (float)
-                temperature               : Temperature measurement (0.01 degrees celsius) (int16_t)
-
-                '''
-                msg = MAVLink_scaled_pressure_message(time_boot_ms, press_abs, press_diff, temperature)
-                msg.pack(self)
-                return msg
-            
-        def scaled_pressure_send(self, time_boot_ms, press_abs, press_diff, temperature):
-                '''
-                The pressure readings for the typical setup of one absolute and
-                differential pressure sensor. The units are as
-                specified in each field.
-
-                time_boot_ms              : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint32_t)
-                press_abs                 : Absolute pressure (hectopascal) (float)
-                press_diff                : Differential pressure 1 (hectopascal) (float)
-                temperature               : Temperature measurement (0.01 degrees celsius) (int16_t)
-
-                '''
-                return self.send(self.scaled_pressure_encode(time_boot_ms, press_abs, press_diff, temperature))
-            
-        def attitude_encode(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed):
-                '''
-                The attitude in the aeronautical frame (right-handed, Z-down, X-front,
-                Y-right).
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-
-                '''
-                msg = MAVLink_attitude_message(time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed)
-                msg.pack(self)
-                return msg
-            
-        def attitude_send(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed):
-                '''
-                The attitude in the aeronautical frame (right-handed, Z-down, X-front,
-                Y-right).
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-
-                '''
-                return self.send(self.attitude_encode(time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed))
-            
-        def attitude_quaternion_encode(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed):
-                '''
-                The attitude in the aeronautical frame (right-handed, Z-down, X-front,
-                Y-right), expressed as quaternion.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                q1                        : Quaternion component 1 (float)
-                q2                        : Quaternion component 2 (float)
-                q3                        : Quaternion component 3 (float)
-                q4                        : Quaternion component 4 (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-
-                '''
-                msg = MAVLink_attitude_quaternion_message(time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed)
-                msg.pack(self)
-                return msg
-            
-        def attitude_quaternion_send(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed):
-                '''
-                The attitude in the aeronautical frame (right-handed, Z-down, X-front,
-                Y-right), expressed as quaternion.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                q1                        : Quaternion component 1 (float)
-                q2                        : Quaternion component 2 (float)
-                q3                        : Quaternion component 3 (float)
-                q4                        : Quaternion component 4 (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-
-                '''
-                return self.send(self.attitude_quaternion_encode(time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed))
-            
-        def local_position_ned_encode(self, time_boot_ms, x, y, z, vx, vy, vz):
-                '''
-                The filtered local position (e.g. fused computer vision and
-                accelerometers). Coordinate frame is right-handed,
-                Z-axis down (aeronautical frame, NED / north-east-down
-                convention)
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                x                         : X Position (float)
-                y                         : Y Position (float)
-                z                         : Z Position (float)
-                vx                        : X Speed (float)
-                vy                        : Y Speed (float)
-                vz                        : Z Speed (float)
-
-                '''
-                msg = MAVLink_local_position_ned_message(time_boot_ms, x, y, z, vx, vy, vz)
-                msg.pack(self)
-                return msg
-            
-        def local_position_ned_send(self, time_boot_ms, x, y, z, vx, vy, vz):
-                '''
-                The filtered local position (e.g. fused computer vision and
-                accelerometers). Coordinate frame is right-handed,
-                Z-axis down (aeronautical frame, NED / north-east-down
-                convention)
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                x                         : X Position (float)
-                y                         : Y Position (float)
-                z                         : Z Position (float)
-                vx                        : X Speed (float)
-                vy                        : Y Speed (float)
-                vz                        : Z Speed (float)
-
-                '''
-                return self.send(self.local_position_ned_encode(time_boot_ms, x, y, z, vx, vy, vz))
-            
-        def global_position_int_encode(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg):
-                '''
-                The filtered global position (e.g. fused GPS and accelerometers). The
-                position is in GPS-frame (right-handed, Z-up). It
-                is designed as scaled integer message since the
-                resolution of float is not sufficient.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                lat                       : Latitude, expressed as * 1E7 (int32_t)
-                lon                       : Longitude, expressed as * 1E7 (int32_t)
-                alt                       : Altitude in meters, expressed as * 1000 (millimeters), above MSL (int32_t)
-                relative_alt              : Altitude above ground in meters, expressed as * 1000 (millimeters) (int32_t)
-                vx                        : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t)
-                vy                        : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t)
-                vz                        : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t)
-                hdg                       : Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t)
-
-                '''
-                msg = MAVLink_global_position_int_message(time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg)
-                msg.pack(self)
-                return msg
-            
-        def global_position_int_send(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg):
-                '''
-                The filtered global position (e.g. fused GPS and accelerometers). The
-                position is in GPS-frame (right-handed, Z-up). It
-                is designed as scaled integer message since the
-                resolution of float is not sufficient.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                lat                       : Latitude, expressed as * 1E7 (int32_t)
-                lon                       : Longitude, expressed as * 1E7 (int32_t)
-                alt                       : Altitude in meters, expressed as * 1000 (millimeters), above MSL (int32_t)
-                relative_alt              : Altitude above ground in meters, expressed as * 1000 (millimeters) (int32_t)
-                vx                        : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t)
-                vy                        : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t)
-                vz                        : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t)
-                hdg                       : Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t)
-
-                '''
-                return self.send(self.global_position_int_encode(time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg))
-            
-        def rc_channels_scaled_encode(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi):
-                '''
-                The scaled values of the RC channels received. (-100%) -10000, (0%) 0,
-                (100%) 10000
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                port                      : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t)
-                chan1_scaled              : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan2_scaled              : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan3_scaled              : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan4_scaled              : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan5_scaled              : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan6_scaled              : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan7_scaled              : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan8_scaled              : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                msg = MAVLink_rc_channels_scaled_message(time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi)
-                msg.pack(self)
-                return msg
-            
-        def rc_channels_scaled_send(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi):
-                '''
-                The scaled values of the RC channels received. (-100%) -10000, (0%) 0,
-                (100%) 10000
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                port                      : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t)
-                chan1_scaled              : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan2_scaled              : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan3_scaled              : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan4_scaled              : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan5_scaled              : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan6_scaled              : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan7_scaled              : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                chan8_scaled              : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                return self.send(self.rc_channels_scaled_encode(time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi))
-            
-        def rc_channels_raw_encode(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi):
-                '''
-                The RAW values of the RC channels received. The standard PPM
-                modulation is as follows: 1000 microseconds: 0%, 2000
-                microseconds: 100%. Individual receivers/transmitters
-                might violate this specification.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                port                      : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t)
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                msg = MAVLink_rc_channels_raw_message(time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi)
-                msg.pack(self)
-                return msg
-            
-        def rc_channels_raw_send(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi):
-                '''
-                The RAW values of the RC channels received. The standard PPM
-                modulation is as follows: 1000 microseconds: 0%, 2000
-                microseconds: 100%. Individual receivers/transmitters
-                might violate this specification.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                port                      : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t)
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                return self.send(self.rc_channels_raw_encode(time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi))
-            
-        def servo_output_raw_encode(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw):
-                '''
-                The RAW values of the servo outputs (for RC input from the remote, use
-                the RC_CHANNELS messages). The standard PPM modulation
-                is as follows: 1000 microseconds: 0%, 2000
-                microseconds: 100%.
-
-                time_usec                 : Timestamp (since UNIX epoch or microseconds since system boot) (uint32_t)
-                port                      : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t)
-                servo1_raw                : Servo output 1 value, in microseconds (uint16_t)
-                servo2_raw                : Servo output 2 value, in microseconds (uint16_t)
-                servo3_raw                : Servo output 3 value, in microseconds (uint16_t)
-                servo4_raw                : Servo output 4 value, in microseconds (uint16_t)
-                servo5_raw                : Servo output 5 value, in microseconds (uint16_t)
-                servo6_raw                : Servo output 6 value, in microseconds (uint16_t)
-                servo7_raw                : Servo output 7 value, in microseconds (uint16_t)
-                servo8_raw                : Servo output 8 value, in microseconds (uint16_t)
-
-                '''
-                msg = MAVLink_servo_output_raw_message(time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw)
-                msg.pack(self)
-                return msg
-            
-        def servo_output_raw_send(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw):
-                '''
-                The RAW values of the servo outputs (for RC input from the remote, use
-                the RC_CHANNELS messages). The standard PPM modulation
-                is as follows: 1000 microseconds: 0%, 2000
-                microseconds: 100%.
-
-                time_usec                 : Timestamp (since UNIX epoch or microseconds since system boot) (uint32_t)
-                port                      : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t)
-                servo1_raw                : Servo output 1 value, in microseconds (uint16_t)
-                servo2_raw                : Servo output 2 value, in microseconds (uint16_t)
-                servo3_raw                : Servo output 3 value, in microseconds (uint16_t)
-                servo4_raw                : Servo output 4 value, in microseconds (uint16_t)
-                servo5_raw                : Servo output 5 value, in microseconds (uint16_t)
-                servo6_raw                : Servo output 6 value, in microseconds (uint16_t)
-                servo7_raw                : Servo output 7 value, in microseconds (uint16_t)
-                servo8_raw                : Servo output 8 value, in microseconds (uint16_t)
-
-                '''
-                return self.send(self.servo_output_raw_encode(time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw))
-            
-        def mission_request_partial_list_encode(self, target_system, target_component, start_index, end_index):
-                '''
-                Request the overall list of MISSIONs from the system/component.
-                http://qgroundcontrol.org/mavlink/waypoint_protocol
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                start_index               : Start index, 0 by default (int16_t)
-                end_index                 : End index, -1 by default (-1: send list to end). Else a valid index of the list (int16_t)
-
-                '''
-                msg = MAVLink_mission_request_partial_list_message(target_system, target_component, start_index, end_index)
-                msg.pack(self)
-                return msg
-            
-        def mission_request_partial_list_send(self, target_system, target_component, start_index, end_index):
-                '''
-                Request the overall list of MISSIONs from the system/component.
-                http://qgroundcontrol.org/mavlink/waypoint_protocol
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                start_index               : Start index, 0 by default (int16_t)
-                end_index                 : End index, -1 by default (-1: send list to end). Else a valid index of the list (int16_t)
-
-                '''
-                return self.send(self.mission_request_partial_list_encode(target_system, target_component, start_index, end_index))
-            
-        def mission_write_partial_list_encode(self, target_system, target_component, start_index, end_index):
-                '''
-                This message is sent to the MAV to write a partial list. If start
-                index == end index, only one item will be transmitted
-                / updated. If the start index is NOT 0 and above the
-                current list size, this request should be REJECTED!
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                start_index               : Start index, 0 by default and smaller / equal to the largest index of the current onboard list. (int16_t)
-                end_index                 : End index, equal or greater than start index. (int16_t)
-
-                '''
-                msg = MAVLink_mission_write_partial_list_message(target_system, target_component, start_index, end_index)
-                msg.pack(self)
-                return msg
-            
-        def mission_write_partial_list_send(self, target_system, target_component, start_index, end_index):
-                '''
-                This message is sent to the MAV to write a partial list. If start
-                index == end index, only one item will be transmitted
-                / updated. If the start index is NOT 0 and above the
-                current list size, this request should be REJECTED!
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                start_index               : Start index, 0 by default and smaller / equal to the largest index of the current onboard list. (int16_t)
-                end_index                 : End index, equal or greater than start index. (int16_t)
-
-                '''
-                return self.send(self.mission_write_partial_list_encode(target_system, target_component, start_index, end_index))
-            
-        def mission_item_encode(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z):
-                '''
-                Message encoding a mission item. This message is emitted to announce
-                the presence of a mission item and to set a mission
-                item on the system. The mission item can be either in
-                x, y, z meters (type: LOCAL) or x:lat, y:lon,
-                z:altitude. Local frame is Z-down, right handed (NED),
-                global frame is Z-up, right handed (ENU).
-                http://qgroundcontrol.org/mavlink/waypoint_protocol
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-                frame                     : The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h (uint8_t)
-                command                   : The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs (uint16_t)
-                current                   : false:0, true:1 (uint8_t)
-                autocontinue              : autocontinue to next wp (uint8_t)
-                param1                    : PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters (float)
-                param2                    : PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float)
-                param3                    : PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float)
-                param4                    : PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH (float)
-                x                         : PARAM5 / local: x position, global: latitude (float)
-                y                         : PARAM6 / y position: global: longitude (float)
-                z                         : PARAM7 / z position: global: altitude (float)
-
-                '''
-                msg = MAVLink_mission_item_message(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z)
-                msg.pack(self)
-                return msg
-            
-        def mission_item_send(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z):
-                '''
-                Message encoding a mission item. This message is emitted to announce
-                the presence of a mission item and to set a mission
-                item on the system. The mission item can be either in
-                x, y, z meters (type: LOCAL) or x:lat, y:lon,
-                z:altitude. Local frame is Z-down, right handed (NED),
-                global frame is Z-up, right handed (ENU).
-                http://qgroundcontrol.org/mavlink/waypoint_protocol
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-                frame                     : The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h (uint8_t)
-                command                   : The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs (uint16_t)
-                current                   : false:0, true:1 (uint8_t)
-                autocontinue              : autocontinue to next wp (uint8_t)
-                param1                    : PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters (float)
-                param2                    : PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float)
-                param3                    : PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float)
-                param4                    : PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH (float)
-                x                         : PARAM5 / local: x position, global: latitude (float)
-                y                         : PARAM6 / y position: global: longitude (float)
-                z                         : PARAM7 / z position: global: altitude (float)
-
-                '''
-                return self.send(self.mission_item_encode(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z))
-            
-        def mission_request_encode(self, target_system, target_component, seq):
-                '''
-                Request the information of the mission item with the sequence number
-                seq. The response of the system to this message should
-                be a MISSION_ITEM message.
-                http://qgroundcontrol.org/mavlink/waypoint_protocol
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_mission_request_message(target_system, target_component, seq)
-                msg.pack(self)
-                return msg
-            
-        def mission_request_send(self, target_system, target_component, seq):
-                '''
-                Request the information of the mission item with the sequence number
-                seq. The response of the system to this message should
-                be a MISSION_ITEM message.
-                http://qgroundcontrol.org/mavlink/waypoint_protocol
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-
-                '''
-                return self.send(self.mission_request_encode(target_system, target_component, seq))
-            
-        def mission_set_current_encode(self, target_system, target_component, seq):
-                '''
-                Set the mission item with sequence number seq as current item. This
-                means that the MAV will continue to this mission item
-                on the shortest path (not following the mission items
-                in-between).
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_mission_set_current_message(target_system, target_component, seq)
-                msg.pack(self)
-                return msg
-            
-        def mission_set_current_send(self, target_system, target_component, seq):
-                '''
-                Set the mission item with sequence number seq as current item. This
-                means that the MAV will continue to this mission item
-                on the shortest path (not following the mission items
-                in-between).
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                seq                       : Sequence (uint16_t)
-
-                '''
-                return self.send(self.mission_set_current_encode(target_system, target_component, seq))
-            
-        def mission_current_encode(self, seq):
-                '''
-                Message that announces the sequence number of the current active
-                mission item. The MAV will fly towards this mission
-                item.
-
-                seq                       : Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_mission_current_message(seq)
-                msg.pack(self)
-                return msg
-            
-        def mission_current_send(self, seq):
-                '''
-                Message that announces the sequence number of the current active
-                mission item. The MAV will fly towards this mission
-                item.
-
-                seq                       : Sequence (uint16_t)
-
-                '''
-                return self.send(self.mission_current_encode(seq))
-            
-        def mission_request_list_encode(self, target_system, target_component):
-                '''
-                Request the overall list of mission items from the system/component.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                msg = MAVLink_mission_request_list_message(target_system, target_component)
-                msg.pack(self)
-                return msg
-            
-        def mission_request_list_send(self, target_system, target_component):
-                '''
-                Request the overall list of mission items from the system/component.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                return self.send(self.mission_request_list_encode(target_system, target_component))
-            
-        def mission_count_encode(self, target_system, target_component, count):
-                '''
-                This message is emitted as response to MISSION_REQUEST_LIST by the MAV
-                and to initiate a write transaction. The GCS can then
-                request the individual mission item based on the
-                knowledge of the total number of MISSIONs.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                count                     : Number of mission items in the sequence (uint16_t)
-
-                '''
-                msg = MAVLink_mission_count_message(target_system, target_component, count)
-                msg.pack(self)
-                return msg
-            
-        def mission_count_send(self, target_system, target_component, count):
-                '''
-                This message is emitted as response to MISSION_REQUEST_LIST by the MAV
-                and to initiate a write transaction. The GCS can then
-                request the individual mission item based on the
-                knowledge of the total number of MISSIONs.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                count                     : Number of mission items in the sequence (uint16_t)
-
-                '''
-                return self.send(self.mission_count_encode(target_system, target_component, count))
-            
-        def mission_clear_all_encode(self, target_system, target_component):
-                '''
-                Delete all mission items at once.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                msg = MAVLink_mission_clear_all_message(target_system, target_component)
-                msg.pack(self)
-                return msg
-            
-        def mission_clear_all_send(self, target_system, target_component):
-                '''
-                Delete all mission items at once.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-
-                '''
-                return self.send(self.mission_clear_all_encode(target_system, target_component))
-            
-        def mission_item_reached_encode(self, seq):
-                '''
-                A certain mission item has been reached. The system will either hold
-                this position (or circle on the orbit) or (if the
-                autocontinue on the WP was set) continue to the next
-                MISSION.
-
-                seq                       : Sequence (uint16_t)
-
-                '''
-                msg = MAVLink_mission_item_reached_message(seq)
-                msg.pack(self)
-                return msg
-            
-        def mission_item_reached_send(self, seq):
-                '''
-                A certain mission item has been reached. The system will either hold
-                this position (or circle on the orbit) or (if the
-                autocontinue on the WP was set) continue to the next
-                MISSION.
-
-                seq                       : Sequence (uint16_t)
-
-                '''
-                return self.send(self.mission_item_reached_encode(seq))
-            
-        def mission_ack_encode(self, target_system, target_component, type):
-                '''
-                Ack message during MISSION handling. The type field states if this
-                message is a positive ack (type=0) or if an error
-                happened (type=non-zero).
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                type                      : See MAV_MISSION_RESULT enum (uint8_t)
-
-                '''
-                msg = MAVLink_mission_ack_message(target_system, target_component, type)
-                msg.pack(self)
-                return msg
-            
-        def mission_ack_send(self, target_system, target_component, type):
-                '''
-                Ack message during MISSION handling. The type field states if this
-                message is a positive ack (type=0) or if an error
-                happened (type=non-zero).
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                type                      : See MAV_MISSION_RESULT enum (uint8_t)
-
-                '''
-                return self.send(self.mission_ack_encode(target_system, target_component, type))
-            
-        def set_gps_global_origin_encode(self, target_system, latitude, longitude, altitude):
-                '''
-                As local MISSIONs exist, the global MISSION reference allows to
-                transform between the local coordinate frame and the
-                global (GPS) coordinate frame. This can be necessary
-                when e.g. in- and outdoor settings are connected and
-                the MAV should move from in- to outdoor.
-
-                target_system             : System ID (uint8_t)
-                latitude                  : global position * 1E7 (int32_t)
-                longitude                 : global position * 1E7 (int32_t)
-                altitude                  : global position * 1000 (int32_t)
-
-                '''
-                msg = MAVLink_set_gps_global_origin_message(target_system, latitude, longitude, altitude)
-                msg.pack(self)
-                return msg
-            
-        def set_gps_global_origin_send(self, target_system, latitude, longitude, altitude):
-                '''
-                As local MISSIONs exist, the global MISSION reference allows to
-                transform between the local coordinate frame and the
-                global (GPS) coordinate frame. This can be necessary
-                when e.g. in- and outdoor settings are connected and
-                the MAV should move from in- to outdoor.
-
-                target_system             : System ID (uint8_t)
-                latitude                  : global position * 1E7 (int32_t)
-                longitude                 : global position * 1E7 (int32_t)
-                altitude                  : global position * 1000 (int32_t)
-
-                '''
-                return self.send(self.set_gps_global_origin_encode(target_system, latitude, longitude, altitude))
-            
-        def gps_global_origin_encode(self, latitude, longitude, altitude):
-                '''
-                Once the MAV sets a new GPS-Local correspondence, this message
-                announces the origin (0,0,0) position
-
-                latitude                  : Latitude (WGS84), expressed as * 1E7 (int32_t)
-                longitude                 : Longitude (WGS84), expressed as * 1E7 (int32_t)
-                altitude                  : Altitude(WGS84), expressed as * 1000 (int32_t)
-
-                '''
-                msg = MAVLink_gps_global_origin_message(latitude, longitude, altitude)
-                msg.pack(self)
-                return msg
-            
-        def gps_global_origin_send(self, latitude, longitude, altitude):
-                '''
-                Once the MAV sets a new GPS-Local correspondence, this message
-                announces the origin (0,0,0) position
-
-                latitude                  : Latitude (WGS84), expressed as * 1E7 (int32_t)
-                longitude                 : Longitude (WGS84), expressed as * 1E7 (int32_t)
-                altitude                  : Altitude(WGS84), expressed as * 1000 (int32_t)
-
-                '''
-                return self.send(self.gps_global_origin_encode(latitude, longitude, altitude))
-            
-        def set_local_position_setpoint_encode(self, target_system, target_component, coordinate_frame, x, y, z, yaw):
-                '''
-                Set the setpoint for a local position controller. This is the position
-                in local coordinates the MAV should fly to. This
-                message is sent by the path/MISSION planner to the
-                onboard position controller. As some MAVs have a
-                degree of freedom in yaw (e.g. all
-                helicopters/quadrotors), the desired yaw angle is part
-                of the message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                coordinate_frame          : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t)
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : Desired yaw angle (float)
-
-                '''
-                msg = MAVLink_set_local_position_setpoint_message(target_system, target_component, coordinate_frame, x, y, z, yaw)
-                msg.pack(self)
-                return msg
-            
-        def set_local_position_setpoint_send(self, target_system, target_component, coordinate_frame, x, y, z, yaw):
-                '''
-                Set the setpoint for a local position controller. This is the position
-                in local coordinates the MAV should fly to. This
-                message is sent by the path/MISSION planner to the
-                onboard position controller. As some MAVs have a
-                degree of freedom in yaw (e.g. all
-                helicopters/quadrotors), the desired yaw angle is part
-                of the message.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                coordinate_frame          : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t)
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : Desired yaw angle (float)
-
-                '''
-                return self.send(self.set_local_position_setpoint_encode(target_system, target_component, coordinate_frame, x, y, z, yaw))
-            
-        def local_position_setpoint_encode(self, coordinate_frame, x, y, z, yaw):
-                '''
-                Transmit the current local setpoint of the controller to other MAVs
-                (collision avoidance) and to the GCS.
-
-                coordinate_frame          : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t)
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : Desired yaw angle (float)
-
-                '''
-                msg = MAVLink_local_position_setpoint_message(coordinate_frame, x, y, z, yaw)
-                msg.pack(self)
-                return msg
-            
-        def local_position_setpoint_send(self, coordinate_frame, x, y, z, yaw):
-                '''
-                Transmit the current local setpoint of the controller to other MAVs
-                (collision avoidance) and to the GCS.
-
-                coordinate_frame          : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t)
-                x                         : x position (float)
-                y                         : y position (float)
-                z                         : z position (float)
-                yaw                       : Desired yaw angle (float)
-
-                '''
-                return self.send(self.local_position_setpoint_encode(coordinate_frame, x, y, z, yaw))
-            
-        def global_position_setpoint_int_encode(self, coordinate_frame, latitude, longitude, altitude, yaw):
-                '''
-                Transmit the current local setpoint of the controller to other MAVs
-                (collision avoidance) and to the GCS.
-
-                coordinate_frame          : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t)
-                latitude                  : WGS84 Latitude position in degrees * 1E7 (int32_t)
-                longitude                 : WGS84 Longitude position in degrees * 1E7 (int32_t)
-                altitude                  : WGS84 Altitude in meters * 1000 (positive for up) (int32_t)
-                yaw                       : Desired yaw angle in degrees * 100 (int16_t)
-
-                '''
-                msg = MAVLink_global_position_setpoint_int_message(coordinate_frame, latitude, longitude, altitude, yaw)
-                msg.pack(self)
-                return msg
-            
-        def global_position_setpoint_int_send(self, coordinate_frame, latitude, longitude, altitude, yaw):
-                '''
-                Transmit the current local setpoint of the controller to other MAVs
-                (collision avoidance) and to the GCS.
-
-                coordinate_frame          : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t)
-                latitude                  : WGS84 Latitude position in degrees * 1E7 (int32_t)
-                longitude                 : WGS84 Longitude position in degrees * 1E7 (int32_t)
-                altitude                  : WGS84 Altitude in meters * 1000 (positive for up) (int32_t)
-                yaw                       : Desired yaw angle in degrees * 100 (int16_t)
-
-                '''
-                return self.send(self.global_position_setpoint_int_encode(coordinate_frame, latitude, longitude, altitude, yaw))
-            
-        def set_global_position_setpoint_int_encode(self, coordinate_frame, latitude, longitude, altitude, yaw):
-                '''
-                Set the current global position setpoint.
-
-                coordinate_frame          : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t)
-                latitude                  : WGS84 Latitude position in degrees * 1E7 (int32_t)
-                longitude                 : WGS84 Longitude position in degrees * 1E7 (int32_t)
-                altitude                  : WGS84 Altitude in meters * 1000 (positive for up) (int32_t)
-                yaw                       : Desired yaw angle in degrees * 100 (int16_t)
-
-                '''
-                msg = MAVLink_set_global_position_setpoint_int_message(coordinate_frame, latitude, longitude, altitude, yaw)
-                msg.pack(self)
-                return msg
-            
-        def set_global_position_setpoint_int_send(self, coordinate_frame, latitude, longitude, altitude, yaw):
-                '''
-                Set the current global position setpoint.
-
-                coordinate_frame          : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t)
-                latitude                  : WGS84 Latitude position in degrees * 1E7 (int32_t)
-                longitude                 : WGS84 Longitude position in degrees * 1E7 (int32_t)
-                altitude                  : WGS84 Altitude in meters * 1000 (positive for up) (int32_t)
-                yaw                       : Desired yaw angle in degrees * 100 (int16_t)
-
-                '''
-                return self.send(self.set_global_position_setpoint_int_encode(coordinate_frame, latitude, longitude, altitude, yaw))
-            
-        def safety_set_allowed_area_encode(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                '''
-                Set a safety zone (volume), which is defined by two corners of a cube.
-                This message can be used to tell the MAV which
-                setpoints/MISSIONs to accept and which to reject.
-                Safety areas are often enforced by national or
-                competition regulations.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                frame                     : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t)
-                p1x                       : x position 1 / Latitude 1 (float)
-                p1y                       : y position 1 / Longitude 1 (float)
-                p1z                       : z position 1 / Altitude 1 (float)
-                p2x                       : x position 2 / Latitude 2 (float)
-                p2y                       : y position 2 / Longitude 2 (float)
-                p2z                       : z position 2 / Altitude 2 (float)
-
-                '''
-                msg = MAVLink_safety_set_allowed_area_message(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z)
-                msg.pack(self)
-                return msg
-            
-        def safety_set_allowed_area_send(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                '''
-                Set a safety zone (volume), which is defined by two corners of a cube.
-                This message can be used to tell the MAV which
-                setpoints/MISSIONs to accept and which to reject.
-                Safety areas are often enforced by national or
-                competition regulations.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                frame                     : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t)
-                p1x                       : x position 1 / Latitude 1 (float)
-                p1y                       : y position 1 / Longitude 1 (float)
-                p1z                       : z position 1 / Altitude 1 (float)
-                p2x                       : x position 2 / Latitude 2 (float)
-                p2y                       : y position 2 / Longitude 2 (float)
-                p2z                       : z position 2 / Altitude 2 (float)
-
-                '''
-                return self.send(self.safety_set_allowed_area_encode(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z))
-            
-        def safety_allowed_area_encode(self, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                '''
-                Read out the safety zone the MAV currently assumes.
-
-                frame                     : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t)
-                p1x                       : x position 1 / Latitude 1 (float)
-                p1y                       : y position 1 / Longitude 1 (float)
-                p1z                       : z position 1 / Altitude 1 (float)
-                p2x                       : x position 2 / Latitude 2 (float)
-                p2y                       : y position 2 / Longitude 2 (float)
-                p2z                       : z position 2 / Altitude 2 (float)
-
-                '''
-                msg = MAVLink_safety_allowed_area_message(frame, p1x, p1y, p1z, p2x, p2y, p2z)
-                msg.pack(self)
-                return msg
-            
-        def safety_allowed_area_send(self, frame, p1x, p1y, p1z, p2x, p2y, p2z):
-                '''
-                Read out the safety zone the MAV currently assumes.
-
-                frame                     : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t)
-                p1x                       : x position 1 / Latitude 1 (float)
-                p1y                       : y position 1 / Longitude 1 (float)
-                p1z                       : z position 1 / Altitude 1 (float)
-                p2x                       : x position 2 / Latitude 2 (float)
-                p2y                       : y position 2 / Longitude 2 (float)
-                p2z                       : z position 2 / Altitude 2 (float)
-
-                '''
-                return self.send(self.safety_allowed_area_encode(frame, p1x, p1y, p1z, p2x, p2y, p2z))
-            
-        def set_roll_pitch_yaw_thrust_encode(self, target_system, target_component, roll, pitch, yaw, thrust):
-                '''
-                Set roll, pitch and yaw.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                roll                      : Desired roll angle in radians (float)
-                pitch                     : Desired pitch angle in radians (float)
-                yaw                       : Desired yaw angle in radians (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                msg = MAVLink_set_roll_pitch_yaw_thrust_message(target_system, target_component, roll, pitch, yaw, thrust)
-                msg.pack(self)
-                return msg
-            
-        def set_roll_pitch_yaw_thrust_send(self, target_system, target_component, roll, pitch, yaw, thrust):
-                '''
-                Set roll, pitch and yaw.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                roll                      : Desired roll angle in radians (float)
-                pitch                     : Desired pitch angle in radians (float)
-                yaw                       : Desired yaw angle in radians (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                return self.send(self.set_roll_pitch_yaw_thrust_encode(target_system, target_component, roll, pitch, yaw, thrust))
-            
-        def set_roll_pitch_yaw_speed_thrust_encode(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust):
-                '''
-                Set roll, pitch and yaw.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                roll_speed                : Desired roll angular speed in rad/s (float)
-                pitch_speed               : Desired pitch angular speed in rad/s (float)
-                yaw_speed                 : Desired yaw angular speed in rad/s (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                msg = MAVLink_set_roll_pitch_yaw_speed_thrust_message(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust)
-                msg.pack(self)
-                return msg
-            
-        def set_roll_pitch_yaw_speed_thrust_send(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust):
-                '''
-                Set roll, pitch and yaw.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                roll_speed                : Desired roll angular speed in rad/s (float)
-                pitch_speed               : Desired pitch angular speed in rad/s (float)
-                yaw_speed                 : Desired yaw angular speed in rad/s (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                return self.send(self.set_roll_pitch_yaw_speed_thrust_encode(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust))
-            
-        def roll_pitch_yaw_thrust_setpoint_encode(self, time_boot_ms, roll, pitch, yaw, thrust):
-                '''
-                Setpoint in roll, pitch, yaw currently active on the system.
-
-                time_boot_ms              : Timestamp in milliseconds since system boot (uint32_t)
-                roll                      : Desired roll angle in radians (float)
-                pitch                     : Desired pitch angle in radians (float)
-                yaw                       : Desired yaw angle in radians (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                msg = MAVLink_roll_pitch_yaw_thrust_setpoint_message(time_boot_ms, roll, pitch, yaw, thrust)
-                msg.pack(self)
-                return msg
-            
-        def roll_pitch_yaw_thrust_setpoint_send(self, time_boot_ms, roll, pitch, yaw, thrust):
-                '''
-                Setpoint in roll, pitch, yaw currently active on the system.
-
-                time_boot_ms              : Timestamp in milliseconds since system boot (uint32_t)
-                roll                      : Desired roll angle in radians (float)
-                pitch                     : Desired pitch angle in radians (float)
-                yaw                       : Desired yaw angle in radians (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                return self.send(self.roll_pitch_yaw_thrust_setpoint_encode(time_boot_ms, roll, pitch, yaw, thrust))
-            
-        def roll_pitch_yaw_speed_thrust_setpoint_encode(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust):
-                '''
-                Setpoint in rollspeed, pitchspeed, yawspeed currently active on the
-                system.
-
-                time_boot_ms              : Timestamp in milliseconds since system boot (uint32_t)
-                roll_speed                : Desired roll angular speed in rad/s (float)
-                pitch_speed               : Desired pitch angular speed in rad/s (float)
-                yaw_speed                 : Desired yaw angular speed in rad/s (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                msg = MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust)
-                msg.pack(self)
-                return msg
-            
-        def roll_pitch_yaw_speed_thrust_setpoint_send(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust):
-                '''
-                Setpoint in rollspeed, pitchspeed, yawspeed currently active on the
-                system.
-
-                time_boot_ms              : Timestamp in milliseconds since system boot (uint32_t)
-                roll_speed                : Desired roll angular speed in rad/s (float)
-                pitch_speed               : Desired pitch angular speed in rad/s (float)
-                yaw_speed                 : Desired yaw angular speed in rad/s (float)
-                thrust                    : Collective thrust, normalized to 0 .. 1 (float)
-
-                '''
-                return self.send(self.roll_pitch_yaw_speed_thrust_setpoint_encode(time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust))
-            
-        def nav_controller_output_encode(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error):
-                '''
-                Outputs of the APM navigation controller. The primary use of this
-                message is to check the response and signs
-                of the controller before actual flight and to assist
-                with tuning controller parameters
-
-                nav_roll                  : Current desired roll in degrees (float)
-                nav_pitch                 : Current desired pitch in degrees (float)
-                nav_bearing               : Current desired heading in degrees (int16_t)
-                target_bearing            : Bearing to current MISSION/target in degrees (int16_t)
-                wp_dist                   : Distance to active MISSION in meters (uint16_t)
-                alt_error                 : Current altitude error in meters (float)
-                aspd_error                : Current airspeed error in meters/second (float)
-                xtrack_error              : Current crosstrack error on x-y plane in meters (float)
-
-                '''
-                msg = MAVLink_nav_controller_output_message(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error)
-                msg.pack(self)
-                return msg
-            
-        def nav_controller_output_send(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error):
-                '''
-                Outputs of the APM navigation controller. The primary use of this
-                message is to check the response and signs
-                of the controller before actual flight and to assist
-                with tuning controller parameters
-
-                nav_roll                  : Current desired roll in degrees (float)
-                nav_pitch                 : Current desired pitch in degrees (float)
-                nav_bearing               : Current desired heading in degrees (int16_t)
-                target_bearing            : Bearing to current MISSION/target in degrees (int16_t)
-                wp_dist                   : Distance to active MISSION in meters (uint16_t)
-                alt_error                 : Current altitude error in meters (float)
-                aspd_error                : Current airspeed error in meters/second (float)
-                xtrack_error              : Current crosstrack error on x-y plane in meters (float)
-
-                '''
-                return self.send(self.nav_controller_output_encode(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error))
-            
-        def state_correction_encode(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr):
-                '''
-                Corrects the systems state by adding an error correction term to the
-                position and velocity, and by rotating the attitude by
-                a correction angle.
-
-                xErr                      : x position error (float)
-                yErr                      : y position error (float)
-                zErr                      : z position error (float)
-                rollErr                   : roll error (radians) (float)
-                pitchErr                  : pitch error (radians) (float)
-                yawErr                    : yaw error (radians) (float)
-                vxErr                     : x velocity (float)
-                vyErr                     : y velocity (float)
-                vzErr                     : z velocity (float)
-
-                '''
-                msg = MAVLink_state_correction_message(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr)
-                msg.pack(self)
-                return msg
-            
-        def state_correction_send(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr):
-                '''
-                Corrects the systems state by adding an error correction term to the
-                position and velocity, and by rotating the attitude by
-                a correction angle.
-
-                xErr                      : x position error (float)
-                yErr                      : y position error (float)
-                zErr                      : z position error (float)
-                rollErr                   : roll error (radians) (float)
-                pitchErr                  : pitch error (radians) (float)
-                yawErr                    : yaw error (radians) (float)
-                vxErr                     : x velocity (float)
-                vyErr                     : y velocity (float)
-                vzErr                     : z velocity (float)
-
-                '''
-                return self.send(self.state_correction_encode(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr))
-            
-        def request_data_stream_encode(self, target_system, target_component, req_stream_id, req_message_rate, start_stop):
-                '''
-                
-
-                target_system             : The target requested to send the message stream. (uint8_t)
-                target_component          : The target requested to send the message stream. (uint8_t)
-                req_stream_id             : The ID of the requested data stream (uint8_t)
-                req_message_rate          : The requested interval between two messages of this type (uint16_t)
-                start_stop                : 1 to start sending, 0 to stop sending. (uint8_t)
-
-                '''
-                msg = MAVLink_request_data_stream_message(target_system, target_component, req_stream_id, req_message_rate, start_stop)
-                msg.pack(self)
-                return msg
-            
-        def request_data_stream_send(self, target_system, target_component, req_stream_id, req_message_rate, start_stop):
-                '''
-                
-
-                target_system             : The target requested to send the message stream. (uint8_t)
-                target_component          : The target requested to send the message stream. (uint8_t)
-                req_stream_id             : The ID of the requested data stream (uint8_t)
-                req_message_rate          : The requested interval between two messages of this type (uint16_t)
-                start_stop                : 1 to start sending, 0 to stop sending. (uint8_t)
-
-                '''
-                return self.send(self.request_data_stream_encode(target_system, target_component, req_stream_id, req_message_rate, start_stop))
-            
-        def data_stream_encode(self, stream_id, message_rate, on_off):
-                '''
-                
-
-                stream_id                 : The ID of the requested data stream (uint8_t)
-                message_rate              : The requested interval between two messages of this type (uint16_t)
-                on_off                    : 1 stream is enabled, 0 stream is stopped. (uint8_t)
-
-                '''
-                msg = MAVLink_data_stream_message(stream_id, message_rate, on_off)
-                msg.pack(self)
-                return msg
-            
-        def data_stream_send(self, stream_id, message_rate, on_off):
-                '''
-                
-
-                stream_id                 : The ID of the requested data stream (uint8_t)
-                message_rate              : The requested interval between two messages of this type (uint16_t)
-                on_off                    : 1 stream is enabled, 0 stream is stopped. (uint8_t)
-
-                '''
-                return self.send(self.data_stream_encode(stream_id, message_rate, on_off))
-            
-        def manual_control_encode(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual):
-                '''
-                
-
-                target                    : The system to be controlled (uint8_t)
-                roll                      : roll (float)
-                pitch                     : pitch (float)
-                yaw                       : yaw (float)
-                thrust                    : thrust (float)
-                roll_manual               : roll control enabled auto:0, manual:1 (uint8_t)
-                pitch_manual              : pitch auto:0, manual:1 (uint8_t)
-                yaw_manual                : yaw auto:0, manual:1 (uint8_t)
-                thrust_manual             : thrust auto:0, manual:1 (uint8_t)
-
-                '''
-                msg = MAVLink_manual_control_message(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual)
-                msg.pack(self)
-                return msg
-            
-        def manual_control_send(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual):
-                '''
-                
-
-                target                    : The system to be controlled (uint8_t)
-                roll                      : roll (float)
-                pitch                     : pitch (float)
-                yaw                       : yaw (float)
-                thrust                    : thrust (float)
-                roll_manual               : roll control enabled auto:0, manual:1 (uint8_t)
-                pitch_manual              : pitch auto:0, manual:1 (uint8_t)
-                yaw_manual                : yaw auto:0, manual:1 (uint8_t)
-                thrust_manual             : thrust auto:0, manual:1 (uint8_t)
-
-                '''
-                return self.send(self.manual_control_encode(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual))
-            
-        def rc_channels_override_encode(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw):
-                '''
-                The RAW values of the RC channels sent to the MAV to override info
-                received from the RC radio. A value of -1 means no
-                change to that channel. A value of 0 means control of
-                that channel should be released back to the RC radio.
-                The standard PPM modulation is as follows: 1000
-                microseconds: 0%, 2000 microseconds: 100%. Individual
-                receivers/transmitters might violate this
-                specification.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-
-                '''
-                msg = MAVLink_rc_channels_override_message(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw)
-                msg.pack(self)
-                return msg
-            
-        def rc_channels_override_send(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw):
-                '''
-                The RAW values of the RC channels sent to the MAV to override info
-                received from the RC radio. A value of -1 means no
-                change to that channel. A value of 0 means control of
-                that channel should be released back to the RC radio.
-                The standard PPM modulation is as follows: 1000
-                microseconds: 0%, 2000 microseconds: 100%. Individual
-                receivers/transmitters might violate this
-                specification.
-
-                target_system             : System ID (uint8_t)
-                target_component          : Component ID (uint8_t)
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-
-                '''
-                return self.send(self.rc_channels_override_encode(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw))
-            
-        def vfr_hud_encode(self, airspeed, groundspeed, heading, throttle, alt, climb):
-                '''
-                Metrics typically displayed on a HUD for fixed wing aircraft
-
-                airspeed                  : Current airspeed in m/s (float)
-                groundspeed               : Current ground speed in m/s (float)
-                heading                   : Current heading in degrees, in compass units (0..360, 0=north) (int16_t)
-                throttle                  : Current throttle setting in integer percent, 0 to 100 (uint16_t)
-                alt                       : Current altitude (MSL), in meters (float)
-                climb                     : Current climb rate in meters/second (float)
-
-                '''
-                msg = MAVLink_vfr_hud_message(airspeed, groundspeed, heading, throttle, alt, climb)
-                msg.pack(self)
-                return msg
-            
-        def vfr_hud_send(self, airspeed, groundspeed, heading, throttle, alt, climb):
-                '''
-                Metrics typically displayed on a HUD for fixed wing aircraft
-
-                airspeed                  : Current airspeed in m/s (float)
-                groundspeed               : Current ground speed in m/s (float)
-                heading                   : Current heading in degrees, in compass units (0..360, 0=north) (int16_t)
-                throttle                  : Current throttle setting in integer percent, 0 to 100 (uint16_t)
-                alt                       : Current altitude (MSL), in meters (float)
-                climb                     : Current climb rate in meters/second (float)
-
-                '''
-                return self.send(self.vfr_hud_encode(airspeed, groundspeed, heading, throttle, alt, climb))
-            
-        def command_long_encode(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7):
-                '''
-                Send a command with up to four parameters to the MAV
-
-                target_system             : System which should execute the command (uint8_t)
-                target_component          : Component which should execute the command, 0 for all components (uint8_t)
-                command                   : Command ID, as defined by MAV_CMD enum. (uint16_t)
-                confirmation              : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t)
-                param1                    : Parameter 1, as defined by MAV_CMD enum. (float)
-                param2                    : Parameter 2, as defined by MAV_CMD enum. (float)
-                param3                    : Parameter 3, as defined by MAV_CMD enum. (float)
-                param4                    : Parameter 4, as defined by MAV_CMD enum. (float)
-                param5                    : Parameter 5, as defined by MAV_CMD enum. (float)
-                param6                    : Parameter 6, as defined by MAV_CMD enum. (float)
-                param7                    : Parameter 7, as defined by MAV_CMD enum. (float)
-
-                '''
-                msg = MAVLink_command_long_message(target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7)
-                msg.pack(self)
-                return msg
-            
-        def command_long_send(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7):
-                '''
-                Send a command with up to four parameters to the MAV
-
-                target_system             : System which should execute the command (uint8_t)
-                target_component          : Component which should execute the command, 0 for all components (uint8_t)
-                command                   : Command ID, as defined by MAV_CMD enum. (uint16_t)
-                confirmation              : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t)
-                param1                    : Parameter 1, as defined by MAV_CMD enum. (float)
-                param2                    : Parameter 2, as defined by MAV_CMD enum. (float)
-                param3                    : Parameter 3, as defined by MAV_CMD enum. (float)
-                param4                    : Parameter 4, as defined by MAV_CMD enum. (float)
-                param5                    : Parameter 5, as defined by MAV_CMD enum. (float)
-                param6                    : Parameter 6, as defined by MAV_CMD enum. (float)
-                param7                    : Parameter 7, as defined by MAV_CMD enum. (float)
-
-                '''
-                return self.send(self.command_long_encode(target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7))
-            
-        def command_ack_encode(self, command, result):
-                '''
-                Report status of a command. Includes feedback wether the command was
-                executed
-
-                command                   : Command ID, as defined by MAV_CMD enum. (uint16_t)
-                result                    : See MAV_RESULT enum (uint8_t)
-
-                '''
-                msg = MAVLink_command_ack_message(command, result)
-                msg.pack(self)
-                return msg
-            
-        def command_ack_send(self, command, result):
-                '''
-                Report status of a command. Includes feedback wether the command was
-                executed
-
-                command                   : Command ID, as defined by MAV_CMD enum. (uint16_t)
-                result                    : See MAV_RESULT enum (uint8_t)
-
-                '''
-                return self.send(self.command_ack_encode(command, result))
-            
-        def hil_state_encode(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc):
-                '''
-                Sent from simulation to autopilot. This packet is useful for high
-                throughput                 applications such as
-                hardware in the loop simulations.
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-                lat                       : Latitude, expressed as * 1E7 (int32_t)
-                lon                       : Longitude, expressed as * 1E7 (int32_t)
-                alt                       : Altitude in meters, expressed as * 1000 (millimeters) (int32_t)
-                vx                        : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t)
-                vy                        : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t)
-                vz                        : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t)
-                xacc                      : X acceleration (mg) (int16_t)
-                yacc                      : Y acceleration (mg) (int16_t)
-                zacc                      : Z acceleration (mg) (int16_t)
-
-                '''
-                msg = MAVLink_hil_state_message(time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc)
-                msg.pack(self)
-                return msg
-            
-        def hil_state_send(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc):
-                '''
-                Sent from simulation to autopilot. This packet is useful for high
-                throughput                 applications such as
-                hardware in the loop simulations.
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll                      : Roll angle (rad) (float)
-                pitch                     : Pitch angle (rad) (float)
-                yaw                       : Yaw angle (rad) (float)
-                rollspeed                 : Roll angular speed (rad/s) (float)
-                pitchspeed                : Pitch angular speed (rad/s) (float)
-                yawspeed                  : Yaw angular speed (rad/s) (float)
-                lat                       : Latitude, expressed as * 1E7 (int32_t)
-                lon                       : Longitude, expressed as * 1E7 (int32_t)
-                alt                       : Altitude in meters, expressed as * 1000 (millimeters) (int32_t)
-                vx                        : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t)
-                vy                        : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t)
-                vz                        : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t)
-                xacc                      : X acceleration (mg) (int16_t)
-                yacc                      : Y acceleration (mg) (int16_t)
-                zacc                      : Z acceleration (mg) (int16_t)
-
-                '''
-                return self.send(self.hil_state_encode(time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc))
-            
-        def hil_controls_encode(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode):
-                '''
-                Sent from autopilot to simulation. Hardware in the loop control
-                outputs
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll_ailerons             : Control output -1 .. 1 (float)
-                pitch_elevator            : Control output -1 .. 1 (float)
-                yaw_rudder                : Control output -1 .. 1 (float)
-                throttle                  : Throttle 0 .. 1 (float)
-                aux1                      : Aux 1, -1 .. 1 (float)
-                aux2                      : Aux 2, -1 .. 1 (float)
-                aux3                      : Aux 3, -1 .. 1 (float)
-                aux4                      : Aux 4, -1 .. 1 (float)
-                mode                      : System mode (MAV_MODE) (uint8_t)
-                nav_mode                  : Navigation mode (MAV_NAV_MODE) (uint8_t)
-
-                '''
-                msg = MAVLink_hil_controls_message(time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode)
-                msg.pack(self)
-                return msg
-            
-        def hil_controls_send(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode):
-                '''
-                Sent from autopilot to simulation. Hardware in the loop control
-                outputs
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                roll_ailerons             : Control output -1 .. 1 (float)
-                pitch_elevator            : Control output -1 .. 1 (float)
-                yaw_rudder                : Control output -1 .. 1 (float)
-                throttle                  : Throttle 0 .. 1 (float)
-                aux1                      : Aux 1, -1 .. 1 (float)
-                aux2                      : Aux 2, -1 .. 1 (float)
-                aux3                      : Aux 3, -1 .. 1 (float)
-                aux4                      : Aux 4, -1 .. 1 (float)
-                mode                      : System mode (MAV_MODE) (uint8_t)
-                nav_mode                  : Navigation mode (MAV_NAV_MODE) (uint8_t)
-
-                '''
-                return self.send(self.hil_controls_encode(time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode))
-            
-        def hil_rc_inputs_raw_encode(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi):
-                '''
-                Sent from simulation to autopilot. The RAW values of the RC channels
-                received. The standard PPM modulation is as follows:
-                1000 microseconds: 0%, 2000 microseconds: 100%.
-                Individual receivers/transmitters might violate this
-                specification.
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-                chan9_raw                 : RC channel 9 value, in microseconds (uint16_t)
-                chan10_raw                : RC channel 10 value, in microseconds (uint16_t)
-                chan11_raw                : RC channel 11 value, in microseconds (uint16_t)
-                chan12_raw                : RC channel 12 value, in microseconds (uint16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                msg = MAVLink_hil_rc_inputs_raw_message(time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi)
-                msg.pack(self)
-                return msg
-            
-        def hil_rc_inputs_raw_send(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi):
-                '''
-                Sent from simulation to autopilot. The RAW values of the RC channels
-                received. The standard PPM modulation is as follows:
-                1000 microseconds: 0%, 2000 microseconds: 100%.
-                Individual receivers/transmitters might violate this
-                specification.
-
-                time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
-                chan1_raw                 : RC channel 1 value, in microseconds (uint16_t)
-                chan2_raw                 : RC channel 2 value, in microseconds (uint16_t)
-                chan3_raw                 : RC channel 3 value, in microseconds (uint16_t)
-                chan4_raw                 : RC channel 4 value, in microseconds (uint16_t)
-                chan5_raw                 : RC channel 5 value, in microseconds (uint16_t)
-                chan6_raw                 : RC channel 6 value, in microseconds (uint16_t)
-                chan7_raw                 : RC channel 7 value, in microseconds (uint16_t)
-                chan8_raw                 : RC channel 8 value, in microseconds (uint16_t)
-                chan9_raw                 : RC channel 9 value, in microseconds (uint16_t)
-                chan10_raw                : RC channel 10 value, in microseconds (uint16_t)
-                chan11_raw                : RC channel 11 value, in microseconds (uint16_t)
-                chan12_raw                : RC channel 12 value, in microseconds (uint16_t)
-                rssi                      : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t)
-
-                '''
-                return self.send(self.hil_rc_inputs_raw_encode(time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi))
-            
-        def optical_flow_encode(self, time_usec, sensor_id, flow_x, flow_y, quality, ground_distance):
-                '''
-                Optical flow from a flow sensor (e.g. optical mouse sensor)
-
-                time_usec                 : Timestamp (UNIX) (uint64_t)
-                sensor_id                 : Sensor ID (uint8_t)
-                flow_x                    : Flow in pixels in x-sensor direction (int16_t)
-                flow_y                    : Flow in pixels in y-sensor direction (int16_t)
-                quality                   : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t)
-                ground_distance           : Ground distance in meters (float)
-
-                '''
-                msg = MAVLink_optical_flow_message(time_usec, sensor_id, flow_x, flow_y, quality, ground_distance)
-                msg.pack(self)
-                return msg
-            
-        def optical_flow_send(self, time_usec, sensor_id, flow_x, flow_y, quality, ground_distance):
-                '''
-                Optical flow from a flow sensor (e.g. optical mouse sensor)
-
-                time_usec                 : Timestamp (UNIX) (uint64_t)
-                sensor_id                 : Sensor ID (uint8_t)
-                flow_x                    : Flow in pixels in x-sensor direction (int16_t)
-                flow_y                    : Flow in pixels in y-sensor direction (int16_t)
-                quality                   : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t)
-                ground_distance           : Ground distance in meters (float)
-
-                '''
-                return self.send(self.optical_flow_encode(time_usec, sensor_id, flow_x, flow_y, quality, ground_distance))
-            
-        def global_vision_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw):
-                '''
-                
-
-                usec                      : Timestamp (milliseconds) (uint64_t)
-                x                         : Global X position (float)
-                y                         : Global Y position (float)
-                z                         : Global Z position (float)
-                roll                      : Roll angle in rad (float)
-                pitch                     : Pitch angle in rad (float)
-                yaw                       : Yaw angle in rad (float)
-
-                '''
-                msg = MAVLink_global_vision_position_estimate_message(usec, x, y, z, roll, pitch, yaw)
-                msg.pack(self)
-                return msg
-            
-        def global_vision_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw):
-                '''
-                
-
-                usec                      : Timestamp (milliseconds) (uint64_t)
-                x                         : Global X position (float)
-                y                         : Global Y position (float)
-                z                         : Global Z position (float)
-                roll                      : Roll angle in rad (float)
-                pitch                     : Pitch angle in rad (float)
-                yaw                       : Yaw angle in rad (float)
-
-                '''
-                return self.send(self.global_vision_position_estimate_encode(usec, x, y, z, roll, pitch, yaw))
-            
-        def vision_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw):
-                '''
-                
-
-                usec                      : Timestamp (milliseconds) (uint64_t)
-                x                         : Global X position (float)
-                y                         : Global Y position (float)
-                z                         : Global Z position (float)
-                roll                      : Roll angle in rad (float)
-                pitch                     : Pitch angle in rad (float)
-                yaw                       : Yaw angle in rad (float)
-
-                '''
-                msg = MAVLink_vision_position_estimate_message(usec, x, y, z, roll, pitch, yaw)
-                msg.pack(self)
-                return msg
-            
-        def vision_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw):
-                '''
-                
-
-                usec                      : Timestamp (milliseconds) (uint64_t)
-                x                         : Global X position (float)
-                y                         : Global Y position (float)
-                z                         : Global Z position (float)
-                roll                      : Roll angle in rad (float)
-                pitch                     : Pitch angle in rad (float)
-                yaw                       : Yaw angle in rad (float)
-
-                '''
-                return self.send(self.vision_position_estimate_encode(usec, x, y, z, roll, pitch, yaw))
-            
-        def vision_speed_estimate_encode(self, usec, x, y, z):
-                '''
-                
-
-                usec                      : Timestamp (milliseconds) (uint64_t)
-                x                         : Global X speed (float)
-                y                         : Global Y speed (float)
-                z                         : Global Z speed (float)
-
-                '''
-                msg = MAVLink_vision_speed_estimate_message(usec, x, y, z)
-                msg.pack(self)
-                return msg
-            
-        def vision_speed_estimate_send(self, usec, x, y, z):
-                '''
-                
-
-                usec                      : Timestamp (milliseconds) (uint64_t)
-                x                         : Global X speed (float)
-                y                         : Global Y speed (float)
-                z                         : Global Z speed (float)
-
-                '''
-                return self.send(self.vision_speed_estimate_encode(usec, x, y, z))
-            
-        def vicon_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw):
-                '''
-                
-
-                usec                      : Timestamp (milliseconds) (uint64_t)
-                x                         : Global X position (float)
-                y                         : Global Y position (float)
-                z                         : Global Z position (float)
-                roll                      : Roll angle in rad (float)
-                pitch                     : Pitch angle in rad (float)
-                yaw                       : Yaw angle in rad (float)
-
-                '''
-                msg = MAVLink_vicon_position_estimate_message(usec, x, y, z, roll, pitch, yaw)
-                msg.pack(self)
-                return msg
-            
-        def vicon_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw):
-                '''
-                
-
-                usec                      : Timestamp (milliseconds) (uint64_t)
-                x                         : Global X position (float)
-                y                         : Global Y position (float)
-                z                         : Global Z position (float)
-                roll                      : Roll angle in rad (float)
-                pitch                     : Pitch angle in rad (float)
-                yaw                       : Yaw angle in rad (float)
-
-                '''
-                return self.send(self.vicon_position_estimate_encode(usec, x, y, z, roll, pitch, yaw))
-            
-        def memory_vect_encode(self, address, ver, type, value):
-                '''
-                Send raw controller memory. The use of this message is discouraged for
-                normal packets, but a quite efficient way for testing
-                new messages and getting experimental debug output.
-
-                address                   : Starting address of the debug variables (uint16_t)
-                ver                       : Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below (uint8_t)
-                type                      : Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 (uint8_t)
-                value                     : Memory contents at specified address (int8_t)
-
-                '''
-                msg = MAVLink_memory_vect_message(address, ver, type, value)
-                msg.pack(self)
-                return msg
-            
-        def memory_vect_send(self, address, ver, type, value):
-                '''
-                Send raw controller memory. The use of this message is discouraged for
-                normal packets, but a quite efficient way for testing
-                new messages and getting experimental debug output.
-
-                address                   : Starting address of the debug variables (uint16_t)
-                ver                       : Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below (uint8_t)
-                type                      : Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 (uint8_t)
-                value                     : Memory contents at specified address (int8_t)
-
-                '''
-                return self.send(self.memory_vect_encode(address, ver, type, value))
-            
-        def debug_vect_encode(self, name, time_usec, x, y, z):
-                '''
-                
-
-                name                      : Name (char)
-                time_usec                 : Timestamp (uint64_t)
-                x                         : x (float)
-                y                         : y (float)
-                z                         : z (float)
-
-                '''
-                msg = MAVLink_debug_vect_message(name, time_usec, x, y, z)
-                msg.pack(self)
-                return msg
-            
-        def debug_vect_send(self, name, time_usec, x, y, z):
-                '''
-                
-
-                name                      : Name (char)
-                time_usec                 : Timestamp (uint64_t)
-                x                         : x (float)
-                y                         : y (float)
-                z                         : z (float)
-
-                '''
-                return self.send(self.debug_vect_encode(name, time_usec, x, y, z))
-            
-        def named_value_float_encode(self, time_boot_ms, name, value):
-                '''
-                Send a key-value pair as float. The use of this message is discouraged
-                for normal packets, but a quite efficient way for
-                testing new messages and getting experimental debug
-                output.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                name                      : Name of the debug variable (char)
-                value                     : Floating point value (float)
-
-                '''
-                msg = MAVLink_named_value_float_message(time_boot_ms, name, value)
-                msg.pack(self)
-                return msg
-            
-        def named_value_float_send(self, time_boot_ms, name, value):
-                '''
-                Send a key-value pair as float. The use of this message is discouraged
-                for normal packets, but a quite efficient way for
-                testing new messages and getting experimental debug
-                output.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                name                      : Name of the debug variable (char)
-                value                     : Floating point value (float)
-
-                '''
-                return self.send(self.named_value_float_encode(time_boot_ms, name, value))
-            
-        def named_value_int_encode(self, time_boot_ms, name, value):
-                '''
-                Send a key-value pair as integer. The use of this message is
-                discouraged for normal packets, but a quite efficient
-                way for testing new messages and getting experimental
-                debug output.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                name                      : Name of the debug variable (char)
-                value                     : Signed integer value (int32_t)
-
-                '''
-                msg = MAVLink_named_value_int_message(time_boot_ms, name, value)
-                msg.pack(self)
-                return msg
-            
-        def named_value_int_send(self, time_boot_ms, name, value):
-                '''
-                Send a key-value pair as integer. The use of this message is
-                discouraged for normal packets, but a quite efficient
-                way for testing new messages and getting experimental
-                debug output.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                name                      : Name of the debug variable (char)
-                value                     : Signed integer value (int32_t)
-
-                '''
-                return self.send(self.named_value_int_encode(time_boot_ms, name, value))
-            
-        def statustext_encode(self, severity, text):
-                '''
-                Status text message. These messages are printed in yellow in the COMM
-                console of QGroundControl. WARNING: They consume quite
-                some bandwidth, so use only for important status and
-                error messages. If implemented wisely, these messages
-                are buffered on the MCU and sent only at a limited
-                rate (e.g. 10 Hz).
-
-                severity                  : Severity of status, 0 = info message, 255 = critical fault (uint8_t)
-                text                      : Status text message, without null termination character (char)
-
-                '''
-                msg = MAVLink_statustext_message(severity, text)
-                msg.pack(self)
-                return msg
-            
-        def statustext_send(self, severity, text):
-                '''
-                Status text message. These messages are printed in yellow in the COMM
-                console of QGroundControl. WARNING: They consume quite
-                some bandwidth, so use only for important status and
-                error messages. If implemented wisely, these messages
-                are buffered on the MCU and sent only at a limited
-                rate (e.g. 10 Hz).
-
-                severity                  : Severity of status, 0 = info message, 255 = critical fault (uint8_t)
-                text                      : Status text message, without null termination character (char)
-
-                '''
-                return self.send(self.statustext_encode(severity, text))
-            
-        def debug_encode(self, time_boot_ms, ind, value):
-                '''
-                Send a debug value. The index is used to discriminate between values.
-                These values show up in the plot of QGroundControl as
-                DEBUG N.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                ind                       : index of debug variable (uint8_t)
-                value                     : DEBUG value (float)
-
-                '''
-                msg = MAVLink_debug_message(time_boot_ms, ind, value)
-                msg.pack(self)
-                return msg
-            
-        def debug_send(self, time_boot_ms, ind, value):
-                '''
-                Send a debug value. The index is used to discriminate between values.
-                These values show up in the plot of QGroundControl as
-                DEBUG N.
-
-                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
-                ind                       : index of debug variable (uint8_t)
-                value                     : DEBUG value (float)
-
-                '''
-                return self.send(self.debug_encode(time_boot_ms, ind, value))
-            
-        def extended_message_encode(self, target_system, target_component, protocol_flags):
-                '''
-                Extended message spacer.
-
-                target_system             : System which should execute the command (uint8_t)
-                target_component          : Component which should execute the command, 0 for all components (uint8_t)
-                protocol_flags            : Retransmission / ACK flags (uint8_t)
-
-                '''
-                msg = MAVLink_extended_message_message(target_system, target_component, protocol_flags)
-                msg.pack(self)
-                return msg
-            
-        def extended_message_send(self, target_system, target_component, protocol_flags):
-                '''
-                Extended message spacer.
-
-                target_system             : System which should execute the command (uint8_t)
-                target_component          : Component which should execute the command, 0 for all components (uint8_t)
-                protocol_flags            : Retransmission / ACK flags (uint8_t)
-
-                '''
-                return self.send(self.extended_message_encode(target_system, target_component, protocol_flags))
-            
diff --git a/mavlink/share/pyshared/pymavlink/mavutil.py b/mavlink/share/pyshared/pymavlink/mavutil.py
deleted file mode 100644
index c0f625214..000000000
--- a/mavlink/share/pyshared/pymavlink/mavutil.py
+++ /dev/null
@@ -1,678 +0,0 @@
-#!/usr/bin/env python
-'''
-mavlink python utility functions
-
-Copyright Andrew Tridgell 2011
-Released under GNU GPL version 3 or later
-'''
-
-import socket, math, struct, time, os, fnmatch, array, sys, errno
-from math import *
-from mavextra import *
-
-if os.getenv('MAVLINK10'):
-    import mavlinkv10 as mavlink
-else:
-    import mavlink
-
-def evaluate_expression(expression, vars):
-    '''evaluation an expression'''
-    try:
-        v = eval(expression, globals(), vars)
-    except NameError:
-        return None
-    return v
-
-def evaluate_condition(condition, vars):
-    '''evaluation a conditional (boolean) statement'''
-    if condition is None:
-        return True
-    v = evaluate_expression(condition, vars)
-    if v is None:
-        return False
-    return v
-
-
-class mavfile(object):
-    '''a generic mavlink port'''
-    def __init__(self, fd, address, source_system=255, notimestamps=False):
-        self.fd = fd
-        self.address = address
-        self.messages = { 'MAV' : self }
-        if mavlink.WIRE_PROTOCOL_VERSION == "1.0":
-            self.messages['HOME'] = mavlink.MAVLink_gps_raw_int_message(0,0,0,0,0,0,0,0,0,0)
-            mavlink.MAVLink_waypoint_message = mavlink.MAVLink_mission_item_message
-        else:
-            self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0)
-        self.params = {}
-        self.mav = None
-        self.target_system = 0
-        self.target_component = 0
-        self.mav = mavlink.MAVLink(self, srcSystem=source_system)
-        self.mav.robust_parsing = True
-        self.logfile = None
-        self.logfile_raw = None
-        self.param_fetch_in_progress = False
-        self.param_fetch_complete = False
-        self.start_time = time.time()
-        self.flightmode = "UNKNOWN"
-        self.timestamp = 0
-        self.message_hooks = []
-        self.idle_hooks = []
-        self.usec = 0
-        self.notimestamps = notimestamps
-        self._timestamp = None
-
-    def recv(self, n=None):
-        '''default recv method'''
-        raise RuntimeError('no recv() method supplied')
-
-    def close(self, n=None):
-        '''default close method'''
-        raise RuntimeError('no close() method supplied')
-
-    def write(self, buf):
-        '''default write method'''
-        raise RuntimeError('no write() method supplied')
-
-    def pre_message(self):
-        '''default pre message call'''
-        return
-
-    def post_message(self, msg):
-        '''default post message call'''
-        msg._timestamp = time.time()
-        type = msg.get_type()
-        self.messages[type] = msg
-
-        if self._timestamp is not None:
-            if self.notimestamps:
-                if 'usec' in msg.__dict__:
-                    self.usec = msg.usec / 1.0e6
-                msg._timestamp = self.usec
-            else:
-                msg._timestamp = self._timestamp
-        
-        self.timestamp = msg._timestamp
-        if type == 'HEARTBEAT':
-            self.target_system = msg.get_srcSystem()
-            self.target_component = msg.get_srcComponent()
-            if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
-                self.flightmode = mode_string_v10(msg)
-        elif type == 'PARAM_VALUE':
-            self.params[str(msg.param_id)] = msg.param_value
-            if msg.param_index+1 == msg.param_count:
-                self.param_fetch_in_progress = False
-                self.param_fetch_complete = True
-        elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':
-            self.flightmode = mode_string_v09(msg)
-        elif type == 'GPS_RAW':
-            if self.messages['HOME'].fix_type < 2:
-                self.messages['HOME'] = msg
-        for hook in self.message_hooks:
-            hook(self, msg)
-
-
-    def recv_msg(self):
-        '''message receive routine'''
-        self.pre_message()
-        while True:
-            n = self.mav.bytes_needed()
-            s = self.recv(n)
-            if len(s) == 0 and len(self.mav.buf) == 0:
-                return None
-            if self.logfile_raw:
-                self.logfile_raw.write(str(s))
-            msg = self.mav.parse_char(s)
-            if msg:
-                self.post_message(msg)
-                return msg
-                
-    def recv_match(self, condition=None, type=None, blocking=False):
-        '''recv the next MAVLink message that matches the given condition'''
-        while True:
-            m = self.recv_msg()
-            if m is None:
-                if blocking:
-                    for hook in self.idle_hooks:
-                        hook(self)
-                    time.sleep(0.01)
-                    continue
-                return None
-            if type is not None and type != m.get_type():
-                continue
-            if not evaluate_condition(condition, self.messages):
-                continue
-            return m
-
-    def setup_logfile(self, logfile, mode='w'):
-        '''start logging to the given logfile, with timestamps'''
-        self.logfile = open(logfile, mode=mode)
-
-    def setup_logfile_raw(self, logfile, mode='w'):
-        '''start logging raw bytes to the given logfile, without timestamps'''
-        self.logfile_raw = open(logfile, mode=mode)
-
-    def wait_heartbeat(self, blocking=True):
-        '''wait for a heartbeat so we know the target system IDs'''
-        return self.recv_match(type='HEARTBEAT', blocking=blocking)
-
-    def param_fetch_all(self):
-        '''initiate fetch of all parameters'''
-        if time.time() - getattr(self, 'param_fetch_start', 0) < 2.0:
-            # don't fetch too often
-            return
-        self.param_fetch_start = time.time()
-        self.param_fetch_in_progress = True
-        self.mav.param_request_list_send(self.target_system, self.target_component)
-
-    def time_since(self, mtype):
-        '''return the time since the last message of type mtype was received'''
-        if not mtype in self.messages:
-            return time.time() - self.start_time
-        return time.time() - self.messages[mtype]._timestamp
-
-    def param_set_send(self, parm_name, parm_value, parm_type=None):
-        '''wrapper for parameter set'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
-            if parm_type == None:
-                parm_type = mavlink.MAV_VAR_FLOAT
-            self.mav.param_set_send(self.target_system, self.target_component,
-                                    parm_name, parm_value, parm_type)
-        else:
-            self.mav.param_set_send(self.target_system, self.target_component,
-                                    parm_name, parm_value)
-
-    def waypoint_request_list_send(self):
-        '''wrapper for waypoint_request_list_send'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
-            self.mav.mission_request_list_send(self.target_system, self.target_component)
-        else:
-            self.mav.waypoint_request_list_send(self.target_system, self.target_component)
-
-    def waypoint_clear_all_send(self):
-        '''wrapper for waypoint_clear_all_send'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
-            self.mav.mission_clear_all_send(self.target_system, self.target_component)
-        else:
-            self.mav.waypoint_clear_all_send(self.target_system, self.target_component)
-
-    def waypoint_request_send(self, seq):
-        '''wrapper for waypoint_request_send'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
-            self.mav.mission_request_send(self.target_system, self.target_component, seq)
-        else:
-            self.mav.waypoint_request_send(self.target_system, self.target_component, seq)
-
-    def waypoint_set_current_send(self, seq):
-        '''wrapper for waypoint_set_current_send'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
-            self.mav.mission_set_current_send(self.target_system, self.target_component, seq)
-        else:
-            self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq)
-
-    def waypoint_count_send(self, seq):
-        '''wrapper for waypoint_count_send'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
-            self.mav.mission_count_send(self.target_system, self.target_component, seq)
-        else:
-            self.mav.waypoint_count_send(self.target_system, self.target_component, seq)
-
-
-class mavserial(mavfile):
-    '''a serial mavlink port'''
-    def __init__(self, device, baud=115200, autoreconnect=False, source_system=255):
-        import serial
-        self.baud = baud
-        self.device = device
-        self.autoreconnect = autoreconnect
-        self.port = serial.Serial(self.device, self.baud, timeout=0,
-                                  dsrdtr=False, rtscts=False, xonxoff=False)
-
-        try:
-            fd = self.port.fileno()
-        except Exception:
-            fd = None
-        mavfile.__init__(self, fd, device, source_system=source_system)
-
-    def close(self):
-        self.port.close()
-
-    def recv(self,n=None):
-        if n is None:
-            n = self.mav.bytes_needed()
-        if self.fd is None:
-            waiting = self.port.inWaiting()
-            if waiting < n:
-                n = waiting
-        return self.port.read(n)
-
-    def write(self, buf):
-        try:
-            return self.port.write(buf)
-        except OSError:
-            if self.autoreconnect:
-                self.reset()
-            return -1
-            
-    def reset(self):
-        import serial
-        self.port.close()
-        while True:
-            try:
-                self.port = serial.Serial(self.device, self.baud, timeout=1,
-                                          dsrdtr=False, rtscts=False, xonxoff=False)
-                try:
-                    self.fd = self.port.fileno()
-                except Exception:
-                    self.fd = None
-                return
-            except Exception:
-                print("Failed to reopen %s" % self.device)
-                time.sleep(1)
-        
-
-class mavudp(mavfile):
-    '''a UDP mavlink socket'''
-    def __init__(self, device, input=True, source_system=255):
-        a = device.split(':')
-        if len(a) != 2:
-            print("UDP ports must be specified as host:port")
-            sys.exit(1)
-        self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-        self.udp_server = input
-        if input:
-            self.port.bind((a[0], int(a[1])))
-        else:
-            self.destination_addr = (a[0], int(a[1]))
-        self.port.setblocking(0)
-        self.last_address = None
-        mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
-
-    def close(self):
-        self.port.close()
-
-    def recv(self,n=None):
-        try:
-            data, self.last_address = self.port.recvfrom(300)
-        except socket.error as e:
-            if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
-                return ""
-            raise
-        return data
-
-    def write(self, buf):
-        try:
-            if self.udp_server:
-                if self.last_address:
-                    self.port.sendto(buf, self.last_address)
-            else:
-                self.port.sendto(buf, self.destination_addr)
-        except socket.error:
-            pass
-
-    def recv_msg(self):
-        '''message receive routine for UDP link'''
-        self.pre_message()
-        s = self.recv()
-        if len(s) == 0:
-            return None
-        msg = self.mav.parse_buffer(s)
-        if msg is not None:
-            for m in msg:
-                self.post_message(m)
-            return msg[0]
-        return None
-
-
-class mavtcp(mavfile):
-    '''a TCP mavlink socket'''
-    def __init__(self, device, source_system=255):
-        a = device.split(':')
-        if len(a) != 2:
-            print("TCP ports must be specified as host:port")
-            sys.exit(1)
-        self.port = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-        self.destination_addr = (a[0], int(a[1]))
-        self.port.connect(self.destination_addr)
-        self.port.setblocking(0)
-        self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
-        mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
-
-    def close(self):
-        self.port.close()
-
-    def recv(self,n=None):
-        if n is None:
-            n = self.mav.bytes_needed()
-        try:
-            data = self.port.recv(n)
-        except socket.error as e:
-            if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
-                return ""
-            raise
-        return data
-
-    def write(self, buf):
-        try:
-            self.port.send(buf)
-        except socket.error:
-            pass
-
-
-class mavlogfile(mavfile):
-    '''a MAVLink logfile reader/writer'''
-    def __init__(self, filename, planner_format=None,
-                 write=False, append=False,
-                 robust_parsing=True, notimestamps=False, source_system=255):
-        self.filename = filename
-        self.writeable = write
-        self.robust_parsing = robust_parsing
-        self.planner_format = planner_format
-        self._two64 = math.pow(2.0, 63)
-        mode = 'rb'
-        if self.writeable:
-            if append:
-                mode = 'ab'
-            else:
-                mode = 'wb'
-        self.f = open(filename, mode)
-        self.filesize = os.path.getsize(filename)
-        self.percent = 0
-        mavfile.__init__(self, None, filename, source_system=source_system, notimestamps=notimestamps)
-        if self.notimestamps:
-            self._timestamp = 0
-        else:
-            self._timestamp = time.time()
-
-    def close(self):
-        self.f.close()
-
-    def recv(self,n=None):
-        if n is None:
-            n = self.mav.bytes_needed()
-        return self.f.read(n)
-
-    def write(self, buf):
-        self.f.write(buf)
-
-    def pre_message(self):
-        '''read timestamp if needed'''
-        # read the timestamp
-        self.percent = (100.0 * self.f.tell()) / self.filesize        
-        if self.notimestamps:
-            return
-        if self.planner_format:
-            tbuf = self.f.read(21)
-            if len(tbuf) != 21 or tbuf[0] != '-' or tbuf[20] != ':':
-                raise RuntimeError('bad planner timestamp %s' % tbuf)
-            hnsec = self._two64 + float(tbuf[0:20])
-            t = hnsec * 1.0e-7         # convert to seconds
-            t -= 719163 * 24 * 60 * 60 # convert to 1970 base
-        else:
-            tbuf = self.f.read(8)
-            if len(tbuf) != 8:
-                return
-            (tusec,) = struct.unpack('>Q', tbuf)
-            t = tusec * 1.0e-6
-        self._timestamp = t
-
-    def post_message(self, msg):
-        '''add timestamp to message'''
-        # read the timestamp
-        super(mavlogfile, self).post_message(msg)
-        if self.planner_format:
-            self.f.read(1) # trailing newline
-        self.timestamp = msg._timestamp
-
-class mavchildexec(mavfile):
-    '''a MAVLink child processes reader/writer'''
-    def __init__(self, filename, source_system=255):
-        from subprocess import Popen, PIPE
-        import fcntl
-        
-        self.filename = filename
-        self.child = Popen(filename, shell=True, stdout=PIPE, stdin=PIPE)
-        self.fd = self.child.stdout.fileno()
-
-        fl = fcntl.fcntl(self.fd, fcntl.F_GETFL)
-        fcntl.fcntl(self.fd, fcntl.F_SETFL, fl | os.O_NONBLOCK)
-
-        fl = fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_GETFL)
-        fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
-
-        mavfile.__init__(self, self.fd, filename, source_system=source_system)
-
-    def close(self):
-        self.child.close()
-
-    def recv(self,n=None):
-        try:
-            x = self.child.stdout.read(1)
-        except Exception:
-            return ''
-        return x
-
-    def write(self, buf):
-        self.child.stdin.write(buf)
-
-
-def mavlink_connection(device, baud=115200, source_system=255,
-                       planner_format=None, write=False, append=False,
-                       robust_parsing=True, notimestamps=False, input=True):
-    '''make a serial or UDP mavlink connection'''
-    if device.startswith('tcp:'):
-        return mavtcp(device[4:], source_system=source_system)
-    if device.startswith('udp:'):
-        return mavudp(device[4:], input=input, source_system=source_system)
-    if device.find(':') != -1 and not device.endswith('log'):
-        return mavudp(device, source_system=source_system, input=input)
-    if os.path.isfile(device):
-        if device.endswith(".elf"):
-            return mavchildexec(device, source_system=source_system)
-        else:
-            return mavlogfile(device, planner_format=planner_format, write=write,
-                              append=append, robust_parsing=robust_parsing, notimestamps=notimestamps,
-                              source_system=source_system)
-    return mavserial(device, baud=baud, source_system=source_system)
-
-class periodic_event(object):
-    '''a class for fixed frequency events'''
-    def __init__(self, frequency):
-        self.frequency = float(frequency)
-        self.last_time = time.time()
-
-    def force(self):
-        '''force immediate triggering'''
-        self.last_time = 0
-        
-    def trigger(self):
-        '''return True if we should trigger now'''
-        tnow = time.time()
-        if self.last_time + (1.0/self.frequency) <= tnow:
-            self.last_time = tnow
-            return True
-        return False
-
-
-try:
-    from curses import ascii
-    have_ascii = True
-except:
-    have_ascii = False
-
-def is_printable(c):
-    '''see if a character is printable'''
-    global have_ascii
-    if have_ascii:
-        return ascii.isprint(c)
-    if isinstance(c, int):
-        ic = c
-    else:
-        ic = ord(c)
-    return ic >= 32 and ic <= 126
-
-def all_printable(buf):
-    '''see if a string is all printable'''
-    for c in buf:
-        if not is_printable(c) and not c in ['\r', '\n', '\t']:
-            return False
-    return True
-
-class SerialPort(object):
-    '''auto-detected serial port'''
-    def __init__(self, device, description=None, hwid=None):
-        self.device = device
-        self.description = description
-        self.hwid = hwid
-
-    def __str__(self):
-        ret = self.device
-        if self.description is not None:
-            ret += " : " + self.description
-        if self.hwid is not None:
-            ret += " : " + self.hwid
-        return ret
-
-def auto_detect_serial_win32(preferred_list=['*']):
-    '''try to auto-detect serial ports on win32'''
-    try:
-        import scanwin32
-        list = sorted(scanwin32.comports())
-    except:
-        return []
-    ret = []
-    for order, port, desc, hwid in list:
-        for preferred in preferred_list:
-            if fnmatch.fnmatch(desc, preferred) or fnmatch.fnmatch(hwid, preferred):
-                ret.append(SerialPort(port, description=desc, hwid=hwid))
-    if len(ret) > 0:
-        return ret
-    # now the rest
-    for order, port, desc, hwid in list:
-        ret.append(SerialPort(port, description=desc, hwid=hwid))
-    return ret
-        
-
-        
-
-def auto_detect_serial_unix(preferred_list=['*']):
-    '''try to auto-detect serial ports on win32'''
-    import glob
-    glist = glob.glob('/dev/ttyS*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyACM*') + glob.glob('/dev/serial/by-id/*')
-    ret = []
-    # try preferred ones first
-    for d in glist:
-        for preferred in preferred_list:
-            if fnmatch.fnmatch(d, preferred):
-                ret.append(SerialPort(d))
-    if len(ret) > 0:
-        return ret
-    # now the rest
-    for d in glist:
-        ret.append(SerialPort(d))
-    return ret
-
-
-
-def auto_detect_serial(preferred_list=['*']):
-    '''try to auto-detect serial port'''
-    # see if 
-    if os.name == 'nt':
-        return auto_detect_serial_win32(preferred_list=preferred_list)
-    return auto_detect_serial_unix(preferred_list=preferred_list)
-
-def mode_string_v09(msg):
-    '''mode string for 0.9 protocol'''
-    mode = msg.mode
-    nav_mode = msg.nav_mode
-
-    MAV_MODE_UNINIT = 0
-    MAV_MODE_MANUAL = 2
-    MAV_MODE_GUIDED = 3
-    MAV_MODE_AUTO = 4
-    MAV_MODE_TEST1 = 5
-    MAV_MODE_TEST2 = 6
-    MAV_MODE_TEST3 = 7
-
-    MAV_NAV_GROUNDED = 0
-    MAV_NAV_LIFTOFF = 1
-    MAV_NAV_HOLD = 2
-    MAV_NAV_WAYPOINT = 3
-    MAV_NAV_VECTOR = 4
-    MAV_NAV_RETURNING = 5
-    MAV_NAV_LANDING = 6
-    MAV_NAV_LOST = 7
-    MAV_NAV_LOITER = 8
-    
-    cmode = (mode, nav_mode)
-    mapping = {
-        (MAV_MODE_UNINIT, MAV_NAV_GROUNDED)  : "INITIALISING",
-        (MAV_MODE_MANUAL, MAV_NAV_VECTOR)    : "MANUAL",
-        (MAV_MODE_TEST3,  MAV_NAV_VECTOR)    : "CIRCLE",
-        (MAV_MODE_GUIDED, MAV_NAV_VECTOR)    : "GUIDED",
-        (MAV_MODE_TEST1,  MAV_NAV_VECTOR)    : "STABILIZE",
-        (MAV_MODE_TEST2,  MAV_NAV_LIFTOFF)   : "FBWA",
-        (MAV_MODE_AUTO,   MAV_NAV_WAYPOINT)  : "AUTO",
-        (MAV_MODE_AUTO,   MAV_NAV_RETURNING) : "RTL",
-        (MAV_MODE_AUTO,   MAV_NAV_LOITER)    : "LOITER",
-        (MAV_MODE_AUTO,   MAV_NAV_LIFTOFF)   : "TAKEOFF",
-        (MAV_MODE_AUTO,   MAV_NAV_LANDING)   : "LANDING",
-        (MAV_MODE_AUTO,   MAV_NAV_HOLD)      : "LOITER",
-        (MAV_MODE_GUIDED, MAV_NAV_VECTOR)    : "GUIDED",
-        (MAV_MODE_GUIDED, MAV_NAV_WAYPOINT)  : "GUIDED",
-        (100,             MAV_NAV_VECTOR)    : "STABILIZE",
-        (101,             MAV_NAV_VECTOR)    : "ACRO",
-        (102,             MAV_NAV_VECTOR)    : "ALT_HOLD",
-        (107,             MAV_NAV_VECTOR)    : "CIRCLE",
-        (109,             MAV_NAV_VECTOR)    : "LAND",
-        }
-    if cmode in mapping:
-        return mapping[cmode]
-    return "Mode(%s,%s)" % cmode
-
-def mode_string_v10(msg):
-    '''mode string for 1.0 protocol, from heartbeat'''
-    if not msg.base_mode & mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED:
-        return "Mode(0x%08x)" % msg.base_mode
-    mapping = {
-        0 : 'MANUAL',
-        1 : 'CIRCLE',
-        2 : 'STABILIZE',
-        5 : 'FBWA',
-        6 : 'FBWB',
-        7 : 'FBWC',
-        10 : 'AUTO',
-        11 : 'RTL',
-        12 : 'LOITER',
-        13 : 'TAKEOFF',
-        14 : 'LAND',
-        15 : 'GUIDED',
-        16 : 'INITIALISING'
-        }
-    if msg.custom_mode in mapping:
-        return mapping[msg.custom_mode]
-    return "Mode(%u)" % msg.custom_mode
-
-    
-
-class x25crc(object):
-    '''x25 CRC - based on checksum.h from mavlink library'''
-    def __init__(self, buf=''):
-        self.crc = 0xffff
-        self.accumulate(buf)
-
-    def accumulate(self, buf):
-        '''add in some more bytes'''
-        bytes = array.array('B')
-        if isinstance(buf, array.array):
-            bytes.extend(buf)
-        else:
-            bytes.fromstring(buf)
-        accum = self.crc
-        for b in bytes:
-            tmp = b ^ (accum & 0xff)
-            tmp = (tmp ^ (tmp<<4)) & 0xFF
-            accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4)
-            accum = accum & 0xFFFF
-        self.crc = accum
diff --git a/mavlink/share/pyshared/pymavlink/mavwp.py b/mavlink/share/pyshared/pymavlink/mavwp.py
deleted file mode 100644
index 6fd1e10e2..000000000
--- a/mavlink/share/pyshared/pymavlink/mavwp.py
+++ /dev/null
@@ -1,200 +0,0 @@
-'''
-module for loading/saving waypoints
-'''
-
-import os
-
-if os.getenv('MAVLINK10'):
-    import mavlinkv10 as mavlink
-else:
-    import mavlink
-
-class MAVWPError(Exception):
-        '''MAVLink WP error class'''
-        def __init__(self, msg):
-            Exception.__init__(self, msg)
-            self.message = msg
-
-class MAVWPLoader(object):
-    '''MAVLink waypoint loader'''
-    def __init__(self, target_system=0, target_component=0):
-        self.wpoints = []
-        self.target_system = target_system
-        self.target_component = target_component
-
-
-    def count(self):
-        '''return number of waypoints'''
-        return len(self.wpoints)
-
-    def wp(self, i):
-        '''return a waypoint'''
-        return self.wpoints[i]
-
-    def add(self, w):
-        '''add a waypoint'''
-        w.seq = self.count()
-        self.wpoints.append(w)
-
-    def remove(self, w):
-        '''remove a waypoint'''
-        self.wpoints.remove(w)
-
-    def clear(self):
-        '''clear waypoint list'''
-        self.wpoints = []
-
-    def _read_waypoint_v100(self, line):
-        '''read a version 100 waypoint'''
-        cmdmap = {
-            2 : mavlink.MAV_CMD_NAV_TAKEOFF,
-            3 : mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
-            4 : mavlink.MAV_CMD_NAV_LAND,
-            24: mavlink.MAV_CMD_NAV_TAKEOFF,
-            26: mavlink.MAV_CMD_NAV_LAND,
-            25: mavlink.MAV_CMD_NAV_WAYPOINT ,
-            27: mavlink.MAV_CMD_NAV_LOITER_UNLIM
-            }
-        a = line.split()
-        if len(a) != 13:
-            raise MAVWPError("invalid waypoint line with %u values" % len(a))
-        w = mavlink.MAVLink_waypoint_message(self.target_system, self.target_component,
-                                             int(a[0]),    # seq
-                                             int(a[1]),    # frame
-                                             int(a[2]),    # action
-                                             int(a[7]),    # current
-                                             int(a[12]),   # autocontinue
-                                             float(a[5]),  # param1,
-                                             float(a[6]),  # param2,
-                                             float(a[3]),  # param3
-                                             float(a[4]),  # param4
-                                             float(a[9]),  # x, latitude
-                                             float(a[8]),  # y, longitude
-                                             float(a[10])  # z
-                                             )
-        if not w.command in cmdmap:
-            raise MAVWPError("Unknown v100 waypoint action %u" % w.command)
-    
-        w.command = cmdmap[w.command]
-        return w
-
-    def _read_waypoint_v110(self, line):
-        '''read a version 110 waypoint'''
-        a = line.split()
-        if len(a) != 12:
-            raise MAVWPError("invalid waypoint line with %u values" % len(a))
-        w = mavlink.MAVLink_waypoint_message(self.target_system, self.target_component,
-                                             int(a[0]),    # seq
-                                             int(a[2]),    # frame
-                                             int(a[3]),    # command
-                                             int(a[1]),    # current
-                                             int(a[11]),   # autocontinue
-                                             float(a[4]),  # param1,
-                                             float(a[5]),  # param2,
-                                             float(a[6]),  # param3
-                                             float(a[7]),  # param4
-                                             float(a[8]),  # x (latitude)
-                                             float(a[9]),  # y (longitude)
-                                             float(a[10])  # z (altitude)
-                                             )
-        return w
-
-
-    def load(self, filename):
-        '''load waypoints from a file.
-        returns number of waypoints loaded'''
-        f = open(filename, mode='r')
-        version_line = f.readline().strip()
-        if version_line == "QGC WPL 100":
-            readfn = self._read_waypoint_v100
-        elif version_line == "QGC WPL 110":
-            readfn = self._read_waypoint_v110
-        else:
-            f.close()
-            raise MAVWPError("Unsupported waypoint format '%s'" % version_line)
-
-        self.clear()
-
-        for line in f:
-            if line.startswith('#'):
-                continue
-            line = line.strip()
-            if not line:
-                continue
-            w = readfn(line)
-            if w is not None:
-                self.add(w)
-        f.close()
-        return len(self.wpoints)
-
-
-    def save(self, filename):
-        '''save waypoints to a file'''
-        f = open(filename, mode='w')
-        f.write("QGC WPL 110\n")
-        for w in self.wpoints:
-            f.write("%u\t%u\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%u\n" % (
-                w.seq, w.current, w.frame, w.command,
-                w.param1, w.param2, w.param3, w.param4,
-                w.x, w.y, w.z, w.autocontinue))
-        f.close()
-
-
-class MAVFenceError(Exception):
-        '''MAVLink fence error class'''
-        def __init__(self, msg):
-            Exception.__init__(self, msg)
-            self.message = msg
-
-class MAVFenceLoader(object):
-    '''MAVLink geo-fence loader'''
-    def __init__(self, target_system=0, target_component=0):
-        self.points = []
-        self.target_system = target_system
-        self.target_component = target_component
-
-    def count(self):
-        '''return number of points'''
-        return len(self.points)
-
-    def point(self, i):
-        '''return a point'''
-        return self.points[i]
-
-    def add(self, p):
-        '''add a point'''
-        self.points.append(p)
-
-    def clear(self):
-        '''clear point list'''
-        self.points = []
-
-    def load(self, filename):
-        '''load points from a file.
-        returns number of points loaded'''
-        f = open(filename, mode='r')
-        self.clear()
-        for line in f:
-            if line.startswith('#'):
-                continue
-            line = line.strip()
-            if not line:
-                continue
-            a = line.split()
-            if len(a) != 2:
-                raise MAVFenceError("invalid fence point line: %s" % line)
-            p = mavlink.MAVLink_fence_point_message(self.target_system, self.target_component,
-                                                    self.count(), 0, float(a[0]), float(a[1]))
-            self.add(p)
-        f.close()
-        for i in range(self.count()):
-            self.points[i].count = self.count()
-        return len(self.points)
-
-
-    def save(self, filename):
-        '''save fence points to a file'''
-        f = open(filename, mode='w')
-        for p in self.points:
-            f.write("%f\t%f\n" % (p.lat, p.lng))
-        f.close()
diff --git a/mavlink/share/pyshared/pymavlink/scanwin32.py b/mavlink/share/pyshared/pymavlink/scanwin32.py
deleted file mode 100644
index f90ed5f4d..000000000
--- a/mavlink/share/pyshared/pymavlink/scanwin32.py
+++ /dev/null
@@ -1,236 +0,0 @@
-#!/usr/bin/env python
-
-# this is taken from the pySerial documentation at
-# http://pyserial.sourceforge.net/examples.html
-import ctypes
-import re
-
-def ValidHandle(value):
-    if value == 0:
-        raise ctypes.WinError()
-    return value
-
-NULL = 0
-HDEVINFO = ctypes.c_int
-BOOL = ctypes.c_int
-CHAR = ctypes.c_char
-PCTSTR = ctypes.c_char_p
-HWND = ctypes.c_uint
-DWORD = ctypes.c_ulong
-PDWORD = ctypes.POINTER(DWORD)
-ULONG = ctypes.c_ulong
-ULONG_PTR = ctypes.POINTER(ULONG)
-#~ PBYTE = ctypes.c_char_p
-PBYTE = ctypes.c_void_p
-
-class GUID(ctypes.Structure):
-    _fields_ = [
-        ('Data1', ctypes.c_ulong),
-        ('Data2', ctypes.c_ushort),
-        ('Data3', ctypes.c_ushort),
-        ('Data4', ctypes.c_ubyte*8),
-    ]
-    def __str__(self):
-        return "{%08x-%04x-%04x-%s-%s}" % (
-            self.Data1,
-            self.Data2,
-            self.Data3,
-            ''.join(["%02x" % d for d in self.Data4[:2]]),
-            ''.join(["%02x" % d for d in self.Data4[2:]]),
-        )
-
-class SP_DEVINFO_DATA(ctypes.Structure):
-    _fields_ = [
-        ('cbSize', DWORD),
-        ('ClassGuid', GUID),
-        ('DevInst', DWORD),
-        ('Reserved', ULONG_PTR),
-    ]
-    def __str__(self):
-        return "ClassGuid:%s DevInst:%s" % (self.ClassGuid, self.DevInst)
-PSP_DEVINFO_DATA = ctypes.POINTER(SP_DEVINFO_DATA)
-
-class SP_DEVICE_INTERFACE_DATA(ctypes.Structure):
-    _fields_ = [
-        ('cbSize', DWORD),
-        ('InterfaceClassGuid', GUID),
-        ('Flags', DWORD),
-        ('Reserved', ULONG_PTR),
-    ]
-    def __str__(self):
-        return "InterfaceClassGuid:%s Flags:%s" % (self.InterfaceClassGuid, self.Flags)
-
-PSP_DEVICE_INTERFACE_DATA = ctypes.POINTER(SP_DEVICE_INTERFACE_DATA)
-
-PSP_DEVICE_INTERFACE_DETAIL_DATA = ctypes.c_void_p
-
-class dummy(ctypes.Structure):
-    _fields_=[("d1", DWORD), ("d2", CHAR)]
-    _pack_ = 1
-SIZEOF_SP_DEVICE_INTERFACE_DETAIL_DATA_A = ctypes.sizeof(dummy)
-
-SetupDiDestroyDeviceInfoList = ctypes.windll.setupapi.SetupDiDestroyDeviceInfoList
-SetupDiDestroyDeviceInfoList.argtypes = [HDEVINFO]
-SetupDiDestroyDeviceInfoList.restype = BOOL
-
-SetupDiGetClassDevs = ctypes.windll.setupapi.SetupDiGetClassDevsA
-SetupDiGetClassDevs.argtypes = [ctypes.POINTER(GUID), PCTSTR, HWND, DWORD]
-SetupDiGetClassDevs.restype = ValidHandle # HDEVINFO
-
-SetupDiEnumDeviceInterfaces = ctypes.windll.setupapi.SetupDiEnumDeviceInterfaces
-SetupDiEnumDeviceInterfaces.argtypes = [HDEVINFO, PSP_DEVINFO_DATA, ctypes.POINTER(GUID), DWORD, PSP_DEVICE_INTERFACE_DATA]
-SetupDiEnumDeviceInterfaces.restype = BOOL
-
-SetupDiGetDeviceInterfaceDetail = ctypes.windll.setupapi.SetupDiGetDeviceInterfaceDetailA
-SetupDiGetDeviceInterfaceDetail.argtypes = [HDEVINFO, PSP_DEVICE_INTERFACE_DATA, PSP_DEVICE_INTERFACE_DETAIL_DATA, DWORD, PDWORD, PSP_DEVINFO_DATA]
-SetupDiGetDeviceInterfaceDetail.restype = BOOL
-
-SetupDiGetDeviceRegistryProperty = ctypes.windll.setupapi.SetupDiGetDeviceRegistryPropertyA
-SetupDiGetDeviceRegistryProperty.argtypes = [HDEVINFO, PSP_DEVINFO_DATA, DWORD, PDWORD, PBYTE, DWORD, PDWORD]
-SetupDiGetDeviceRegistryProperty.restype = BOOL
-
-
-GUID_CLASS_COMPORT = GUID(0x86e0d1e0L, 0x8089, 0x11d0,
-    (ctypes.c_ubyte*8)(0x9c, 0xe4, 0x08, 0x00, 0x3e, 0x30, 0x1f, 0x73))
-
-DIGCF_PRESENT = 2
-DIGCF_DEVICEINTERFACE = 16
-INVALID_HANDLE_VALUE = 0
-ERROR_INSUFFICIENT_BUFFER = 122
-SPDRP_HARDWAREID = 1
-SPDRP_FRIENDLYNAME = 12
-SPDRP_LOCATION_INFORMATION = 13
-ERROR_NO_MORE_ITEMS = 259
-
-def comports(available_only=True):
-    """This generator scans the device registry for com ports and yields
-    (order, port, desc, hwid).  If available_only is true only return currently
-    existing ports. Order is a helper to get sorted lists. it can be ignored
-    otherwise."""
-    flags = DIGCF_DEVICEINTERFACE
-    if available_only:
-        flags |= DIGCF_PRESENT
-    g_hdi = SetupDiGetClassDevs(ctypes.byref(GUID_CLASS_COMPORT), None, NULL, flags);
-    #~ for i in range(256):
-    for dwIndex in range(256):
-        did = SP_DEVICE_INTERFACE_DATA()
-        did.cbSize = ctypes.sizeof(did)
-
-        if not SetupDiEnumDeviceInterfaces(
-            g_hdi,
-            None,
-            ctypes.byref(GUID_CLASS_COMPORT),
-            dwIndex,
-            ctypes.byref(did)
-        ):
-            if ctypes.GetLastError() != ERROR_NO_MORE_ITEMS:
-                raise ctypes.WinError()
-            break
-
-        dwNeeded = DWORD()
-        # get the size
-        if not SetupDiGetDeviceInterfaceDetail(
-            g_hdi,
-            ctypes.byref(did),
-            None, 0, ctypes.byref(dwNeeded),
-            None
-        ):
-            # Ignore ERROR_INSUFFICIENT_BUFFER
-            if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER:
-                raise ctypes.WinError()
-        # allocate buffer
-        class SP_DEVICE_INTERFACE_DETAIL_DATA_A(ctypes.Structure):
-            _fields_ = [
-                ('cbSize', DWORD),
-                ('DevicePath', CHAR*(dwNeeded.value - ctypes.sizeof(DWORD))),
-            ]
-            def __str__(self):
-                return "DevicePath:%s" % (self.DevicePath,)
-        idd = SP_DEVICE_INTERFACE_DETAIL_DATA_A()
-        idd.cbSize = SIZEOF_SP_DEVICE_INTERFACE_DETAIL_DATA_A
-        devinfo = SP_DEVINFO_DATA()
-        devinfo.cbSize = ctypes.sizeof(devinfo)
-        if not SetupDiGetDeviceInterfaceDetail(
-            g_hdi,
-            ctypes.byref(did),
-            ctypes.byref(idd), dwNeeded, None,
-            ctypes.byref(devinfo)
-        ):
-            raise ctypes.WinError()
-
-        # hardware ID
-        szHardwareID = ctypes.create_string_buffer(250)
-        if not SetupDiGetDeviceRegistryProperty(
-            g_hdi,
-            ctypes.byref(devinfo),
-            SPDRP_HARDWAREID,
-            None,
-            ctypes.byref(szHardwareID), ctypes.sizeof(szHardwareID) - 1,
-            None
-        ):
-            # Ignore ERROR_INSUFFICIENT_BUFFER
-            if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER:
-                raise ctypes.WinError()
-
-        # friendly name
-        szFriendlyName = ctypes.create_string_buffer(1024)
-        if not SetupDiGetDeviceRegistryProperty(
-            g_hdi,
-            ctypes.byref(devinfo),
-            SPDRP_FRIENDLYNAME,
-            None,
-            ctypes.byref(szFriendlyName), ctypes.sizeof(szFriendlyName) - 1,
-            None
-        ):
-            # Ignore ERROR_INSUFFICIENT_BUFFER
-            if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER:
-                #~ raise ctypes.WinError()
-                # not getting friendly name for com0com devices, try something else
-                szFriendlyName = ctypes.create_string_buffer(1024)
-                if SetupDiGetDeviceRegistryProperty(
-                    g_hdi,
-                    ctypes.byref(devinfo),
-                    SPDRP_LOCATION_INFORMATION,
-                    None,
-                    ctypes.byref(szFriendlyName), ctypes.sizeof(szFriendlyName) - 1,
-                    None
-                ):
-                    port_name = "\\\\.\\" + szFriendlyName.value
-                    order = None
-                else:
-                    port_name = szFriendlyName.value
-                    order = None
-        else:
-            try:
-                m = re.search(r"\((.*?(\d+))\)", szFriendlyName.value)
-                #~ print szFriendlyName.value, m.groups()
-                port_name = m.group(1)
-                order = int(m.group(2))
-            except AttributeError, msg:
-                port_name = szFriendlyName.value
-                order = None
-        yield order, port_name, szFriendlyName.value, szHardwareID.value
-
-    SetupDiDestroyDeviceInfoList(g_hdi)
-
-
-if __name__ == '__main__':
-    import serial
-    print "-"*78
-    print "Serial ports"
-    print "-"*78
-    for order, port, desc, hwid in sorted(comports()):
-        print "%-10s: %s (%s) ->" % (port, desc, hwid),
-        try:
-            serial.Serial(port) # test open
-        except serial.serialutil.SerialException:
-            print "can't be openend"
-        else:
-            print "Ready"
-    print
-    # list of all ports the system knows
-    print "-"*78
-    print "All serial ports (registry)"
-    print "-"*78
-    for order, port, desc, hwid in sorted(comports(False)):
-        print "%-10s: %s (%s)" % (port, desc, hwid)
diff --git a/mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif b/mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif
deleted file mode 100644
index 3cce0867b..000000000
--- a/mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif
+++ /dev/null
diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif
deleted file mode 100644
index 4dc439b78..000000000
--- a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif
+++ /dev/null
diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif
deleted file mode 100644
index ec4136399..000000000
--- a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif
+++ /dev/null
diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif
deleted file mode 100644
index 306271a89..000000000
--- a/mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif
+++ /dev/null
diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif
deleted file mode 100644
index 23fcf6d82..000000000
--- a/mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif
+++ /dev/null
diff --git a/mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif b/mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif
deleted file mode 100644
index 3439c54fd..000000000
--- a/mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif
+++ /dev/null
diff --git a/mavlink/share/pyshared/pymavlink/tools/images/player_end.gif b/mavlink/share/pyshared/pymavlink/tools/images/player_end.gif
deleted file mode 100644
index 9496957d2..000000000
--- a/mavlink/share/pyshared/pymavlink/tools/images/player_end.gif
+++ /dev/null
diff --git a/mavlink/share/pyshared/pymavlink/tools/mavplayback.py b/mavlink/share/pyshared/pymavlink/tools/mavplayback.py
deleted file mode 100644
index 033746697..000000000
--- a/mavlink/share/pyshared/pymavlink/tools/mavplayback.py
+++ /dev/null
@@ -1,246 +0,0 @@
-#!/usr/bin/env python
-
-'''
-play back a mavlink log as a FlightGear FG NET stream, and as a
-realtime mavlink stream
-
-Useful for visualising flights
-'''
-
-import sys, time, os, struct
-import Tkinter
-
-# allow import from the parent directory, where mavlink.py is
-sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
-
-import fgFDM
-
-from optparse import OptionParser
-parser = OptionParser("mavplayback.py [options]")
-
-parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format")
-parser.add_option("--condition",dest="condition", default=None, help="select packets by condition")
-parser.add_option("--gpsalt", action='store_true', default=False, help="Use GPS altitude")
-parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
-parser.add_option("--out",   help="MAVLink output port (IP:port)",
-                  action='append', default=['127.0.0.1:14550'])
-parser.add_option("--fgout", action='append', default=['127.0.0.1:5503'],
-                  help="flightgear FDM NET output (IP:port)")
-parser.add_option("--baudrate", type='int', default=57600, help='baud rate')
-(opts, args) = parser.parse_args()
-
-if opts.mav10:
-    os.environ['MAVLINK10'] = '1'
-import mavutil
-
-if len(args) < 1:
-    parser.print_help()
-    sys.exit(1)
-
-filename = args[0]
-
-
-def LoadImage(filename):
-    '''return an image from the images/ directory'''
-    app_dir = os.path.dirname(os.path.realpath(__file__))
-    path = os.path.join(app_dir, 'files/images', filename)
-    return Tkinter.PhotoImage(file=path)
-
-
-class App():
-    def __init__(self, filename):
-        self.root = Tkinter.Tk()
-
-        self.filesize = os.path.getsize(filename)
-        self.filepos = 0.0
-
-        self.mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner,
-                                               robust_parsing=True)
-        self.mout = []
-        for m in opts.out:
-            self.mout.append(mavutil.mavlink_connection(m, input=False, baud=opts.baudrate))
-
-        self.fgout = []
-        for f in opts.fgout:
-            self.fgout.append(mavutil.mavudp(f, input=False))
-    
-        self.fdm = fgFDM.fgFDM()
-
-        self.msg = self.mlog.recv_match(condition=opts.condition)
-        if self.msg is None:
-            sys.exit(1)
-        self.last_timestamp = getattr(self.msg, '_timestamp')
-
-        self.paused = False
-
-        self.topframe = Tkinter.Frame(self.root)
-        self.topframe.pack(side=Tkinter.TOP)
-
-        self.frame = Tkinter.Frame(self.root)
-        self.frame.pack(side=Tkinter.LEFT)
-
-        self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01,
-                                    orient=Tkinter.HORIZONTAL, command=self.slew)
-        self.slider.pack(side=Tkinter.LEFT)
-
-        self.clock = Tkinter.Label(self.topframe,text="")
-        self.clock.pack(side=Tkinter.RIGHT)
-
-        self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3)
-        self.playback.pack(side=Tkinter.BOTTOM)
-        self.playback.delete(0, "end")
-        self.playback.insert(0, 1)
-
-        self.buttons = {}
-        self.button('quit', 'gtk-quit.gif', self.frame.quit)
-        self.button('pause', 'media-playback-pause.gif', self.pause)
-        self.button('rewind', 'media-seek-backward.gif', self.rewind)
-        self.button('forward', 'media-seek-forward.gif', self.forward)
-        self.button('status', 'Status', self.status)
-        self.flightmode = Tkinter.Label(self.frame,text="")
-        self.flightmode.pack(side=Tkinter.RIGHT)
-
-        self.next_message()
-        self.root.mainloop()
-
-    def button(self, name, filename, command):
-        '''add a button'''
-        try:
-            img = LoadImage(filename)
-            b = Tkinter.Button(self.frame, image=img, command=command)
-            b.image = img
-        except Exception:
-            b = Tkinter.Button(self.frame, text=filename, command=command)
-        b.pack(side=Tkinter.LEFT)
-        self.buttons[name] = b
-        
-
-    def pause(self):
-        '''pause playback'''
-        self.paused = not self.paused
-
-    def rewind(self):
-        '''rewind 10%'''
-        pos = int(self.mlog.f.tell() - 0.1*self.filesize)
-        if pos < 0:
-            pos = 0
-        self.mlog.f.seek(pos)
-        self.find_message()
-
-    def forward(self):
-        '''forward 10%'''
-        pos = int(self.mlog.f.tell() + 0.1*self.filesize)
-        if pos > self.filesize:
-            pos = self.filesize - 2048
-        self.mlog.f.seek(pos)
-        self.find_message()
-
-    def status(self):
-        '''show status'''
-        for m in sorted(self.mlog.messages.keys()):
-            print(str(self.mlog.messages[m]))
-        
-
-
-    def find_message(self):
-        '''find the next valid message'''
-        while True:
-            self.msg = self.mlog.recv_match(condition=opts.condition)
-            if self.msg is not None and self.msg.get_type() != 'BAD_DATA':
-                break
-            if self.mlog.f.tell() > self.filesize - 10:
-                self.paused = True
-                break
-        self.last_timestamp = getattr(self.msg, '_timestamp')
-
-    def slew(self, value):
-        '''move to a given position in the file'''
-        if float(value) != self.filepos:
-            pos = float(value) * self.filesize
-            self.mlog.f.seek(int(pos))
-            self.find_message()
-
-
-    def next_message(self):
-        '''called as each msg is ready'''
-        
-        msg = self.msg
-        if msg is None:
-            self.paused = True
-
-        if self.paused:
-            self.root.after(100, self.next_message)
-            return
-
-        speed = float(self.playback.get())
-        timestamp = getattr(msg, '_timestamp')
-
-        now = time.strftime("%H:%M:%S", time.localtime(timestamp))
-        self.clock.configure(text=now)
-
-        if speed == 0.0:
-            self.root.after(200, self.next_message)
-        else:
-            self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message)
-        self.last_timestamp = timestamp
-
-        while True:
-            self.msg = self.mlog.recv_match(condition=opts.condition)
-            if self.msg is None and self.mlog.f.tell() > self.filesize - 10:
-                self.paused = True
-                return
-            if self.msg is not None and self.msg.get_type() != "BAD_DATA":
-                break
-        
-        pos = float(self.mlog.f.tell()) / self.filesize
-        self.slider.set(pos)
-        self.filepos = self.slider.get()
-
-        if msg.get_type() != "BAD_DATA":
-            for m in self.mout:
-                m.write(msg.get_msgbuf().tostring())
-
-        if msg.get_type() == "GPS_RAW":
-            self.fdm.set('latitude', msg.lat, units='degrees')
-            self.fdm.set('longitude', msg.lon, units='degrees')
-            if opts.gpsalt:
-                self.fdm.set('altitude', msg.alt, units='meters')
-
-        if msg.get_type() == "VFR_HUD":
-            if not opts.gpsalt:
-                self.fdm.set('altitude', msg.alt, units='meters')
-            self.fdm.set('num_engines', 1)
-            self.fdm.set('vcas', msg.airspeed, units='mps')
-
-        if msg.get_type() == "ATTITUDE":
-            self.fdm.set('phi', msg.roll, units='radians')
-            self.fdm.set('theta', msg.pitch, units='radians')
-            self.fdm.set('psi', msg.yaw, units='radians')
-            self.fdm.set('phidot', msg.rollspeed, units='rps')
-            self.fdm.set('thetadot', msg.pitchspeed, units='rps')
-            self.fdm.set('psidot', msg.yawspeed, units='rps')
-
-        if msg.get_type() == "RC_CHANNELS_SCALED":
-            self.fdm.set("right_aileron", msg.chan1_scaled*0.0001)
-            self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001)
-            self.fdm.set("rudder",        msg.chan4_scaled*0.0001)
-            self.fdm.set("elevator",      msg.chan2_scaled*0.0001)
-            self.fdm.set('rpm',           msg.chan3_scaled*0.01)
-
-        if msg.get_type() == 'STATUSTEXT':
-            print("APM: %s" % msg.text)
-
-        if msg.get_type() == 'SYS_STATUS':
-            self.flightmode.configure(text=self.mlog.flightmode)
-
-        if msg.get_type() == "BAD_DATA":
-            if mavutil.all_printable(msg.data):
-                sys.stdout.write(msg.data)
-                sys.stdout.flush()
-
-        if self.fdm.get('latitude') != 0:
-            for f in self.fgout:
-                f.write(self.fdm.pack())
-
-
-app=App(filename)
diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h
new file mode 100644
index 000000000..8705c1bc2
--- /dev/null
+++ b/nuttx-configs/aerocore/include/board.h
@@ -0,0 +1,263 @@
+/************************************************************************************
+ * configs/aerocore/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The AeroCore uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c:
+ *   System Clock source           : PLL (HSE)
+ *   SYSCLK(Hz)                    : 168000000    Determined by PLL configuration
+ *   HCLK(Hz)                      : 168000000    (STM32_RCC_CFGR_HPRE)
+ *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
+ *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
+ *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
+ *   HSE Frequency(Hz)             : 24000000     (STM32_BOARD_XTAL)
+ *   PLLM                          : 24           (STM32_PLLCFG_PLLM)
+ *   PLLN                          : 336          (STM32_PLLCFG_PLLN)
+ *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
+ *   PLLQ                          : 7            (STM32_PLLCFG_PPQ)
+ *   Main regulator output voltage : Scale1 mode  Needed for high speed SYSCLK
+ *   Flash Latency(WS)             : 5
+ *   Prefetch Buffer               : OFF
+ *   Instruction cache             : ON
+ *   Data cache                    : ON
+ *   Require 48MHz for USB OTG FS, : Enabled
+ *   SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 24MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (24,000,000 / 24) * 336
+ *         = 336,000,000
+ * SYSCLK  = PLL_VCO / PLLP
+ *         = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ *         =  PLL_VCO / PLLQ
+ *         = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(24)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY  168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx. 
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ */
+/* USART1 on PB[6,7]: GPS */
+#define GPIO_USART1_RX	GPIO_USART1_RX_2
+#define GPIO_USART1_TX	GPIO_USART1_TX_2
+
+/* USART2 on PD[5,6]: J5 Breakout */
+#define GPIO_USART2_RX	GPIO_USART2_RX_2
+#define GPIO_USART2_TX	GPIO_USART2_TX_2
+#define GPIO_USART2_CTS	0 // unused
+#define GPIO_USART2_RTS	0 // unused
+
+/* USART3 on PD[8,9]: to DuoVero UART2 */
+#define GPIO_USART3_RX	GPIO_USART3_RX_3
+#define GPIO_USART3_TX	GPIO_USART3_TX_3
+#define GPIO_USART3_CTS	0 // unused
+#define GPIO_USART3_RTS	0 // unused
+
+/* UART7 on PE[78]: J7 Breakout */
+#define GPIO_UART7_RX	GPIO_UART7_RX_1
+#define GPIO_UART7_TX	GPIO_UART7_TX_1
+
+/*
+ * UART8 on PE[0-1]: System Console on Port C of USB (J7)
+ * No alternate pin config
+*/
+
+/* USART[1,6] require a RX DMA configuration */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+
+/* PB[10-11]: I2C2 is broken out on J9 header */
+#define GPIO_I2C2_SCL		GPIO_I2C2_SCL_1
+#define GPIO_I2C2_SDA		GPIO_I2C2_SDA_1
+#define GPIO_I2C2_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+
+/*
+ * SPI
+ */
+/* PA[4-7] SPI1 broken out on J12 */ 
+#define GPIO_SPI1_NSS	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */
+#define GPIO_SPI1_SCK	(GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+
+/* PB[12-15]: SPI2 connected to DuoVero SPI1 */
+#define GPIO_SPI2_NSS	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) /* should be GPIO_SPI2_NSS_2 but use as a GPIO */
+#define GPIO_SPI2_SCK	(GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MISO	(GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MOSI	(GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
+
+/* PC[10-12]: SPI3 connected to onboard sensors */
+#define GPIO_SPI3_SCK	(GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_MISO	(GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_MOSI	(GPIO_SPI3_MOSI_2|GPIO_SPEED_50MHz)
+
+/* PE[11-14]: SPI4 connected to FRAM */
+#define GPIO_SPI4_NSS	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN11) /* should be GPIO_SPI4_NSS_2 but use as a GPIO */
+#define GPIO_SPI4_SCK	(GPIO_SPI4_SCK_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI4_MISO	(GPIO_SPI4_MISO_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI4_MOSI	(GPIO_SPI4_MOSI_2|GPIO_SPEED_50MHz)
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */
diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/nuttx-configs/aerocore/include/nsh_romfsimg.h
index 21661622a..15e4e7a8d 100644
--- a/src/lib/mathlib/math/arm/Matrix.cpp
+++ b/nuttx-configs/aerocore/include/nsh_romfsimg.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,9 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file Matrix.cpp
+ * nsh_romfsetc.h
  *
- * matrix code
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
  */
 
-#include "Matrix.hpp"
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs
new file mode 100644
index 000000000..c7a1b71bb
--- /dev/null
+++ b/nuttx-configs/aerocore/nsh/Make.defs
@@ -0,0 +1,179 @@
+############################################################################
+# configs/aerocore/nsh/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -O3
+ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
+			   -mthumb \
+			   -march=armv7e-m \
+			   -mfpu=fpv4-sp-d16 \
+			   -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+INSTRUMENTATIONDEFINES	 = -finstrument-functions \
+			   -ffixed-r10
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS		+= $(LIBM)
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain 
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+   			   -funsafe-math-optimizations \
+   			   -fno-builtin-printf \
+   			   -ffunction-sections \
+   			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wfloat-equal \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wmissing-declarations \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wmissing-prototypes \
+			   -Wnested-externs \
+			   -Wunsuffixed-float-constants
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-psabi
+ARCHDEFINES		 =
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
diff --git a/nuttx-configs/aerocore/nsh/appconfig b/nuttx-configs/aerocore/nsh/appconfig
new file mode 100644
index 000000000..2850dac06
--- /dev/null
+++ b/nuttx-configs/aerocore/nsh/appconfig
@@ -0,0 +1,42 @@
+############################################################################
+# configs/aerocore/nsh/appconfig
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+# Path to example in apps/examples containing the user_start entry point
+
+CONFIGURED_APPS	+= examples/nsh
+
+# The NSH application library
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += system/readline
diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig
new file mode 100644
index 000000000..8d0bae7b9
--- /dev/null
+++ b/nuttx-configs/aerocore/nsh/defconfig
@@ -0,0 +1,950 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+CONFIG_HOST_LINUX=y
+# CONFIG_HOST_OSX is not set
+# CONFIG_HOST_WINDOWS is not set
+# CONFIG_HOST_OTHER is not set
+
+#
+# Build Configuration
+#
+CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+# CONFIG_INTELHEX_BINARY is not set
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+CONFIG_ARCH_MATH_H=y
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+# CONFIG_DEBUG_SYMBOLS is not set
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_KL is not set
+# CONFIG_ARCH_CHIP_LM is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_NUC1XX is not set
+# CONFIG_ARCH_CHIP_SAM34 is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_ARCH_HAVE_FPU=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set
+# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_SERIAL_TERMIOS=y
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32L151C6 is not set
+# CONFIG_ARCH_CHIP_STM32L151C8 is not set
+# CONFIG_ARCH_CHIP_STM32L151CB is not set
+# CONFIG_ARCH_CHIP_STM32L151R6 is not set
+# CONFIG_ARCH_CHIP_STM32L151R8 is not set
+# CONFIG_ARCH_CHIP_STM32L151RB is not set
+# CONFIG_ARCH_CHIP_STM32L151V6 is not set
+# CONFIG_ARCH_CHIP_STM32L151V8 is not set
+# CONFIG_ARCH_CHIP_STM32L151VB is not set
+# CONFIG_ARCH_CHIP_STM32L152C6 is not set
+# CONFIG_ARCH_CHIP_STM32L152C8 is not set
+# CONFIG_ARCH_CHIP_STM32L152CB is not set
+# CONFIG_ARCH_CHIP_STM32L152R6 is not set
+# CONFIG_ARCH_CHIP_STM32L152R8 is not set
+# CONFIG_ARCH_CHIP_STM32L152RB is not set
+# CONFIG_ARCH_CHIP_STM32L152V6 is not set
+# CONFIG_ARCH_CHIP_STM32L152V8 is not set
+# CONFIG_ARCH_CHIP_STM32L152VB is not set
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F103C4 is not set
+# CONFIG_ARCH_CHIP_STM32F103C8 is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F302CB is not set
+# CONFIG_ARCH_CHIP_STM32F302CC is not set
+# CONFIG_ARCH_CHIP_STM32F302RB is not set
+# CONFIG_ARCH_CHIP_STM32F302RC is not set
+# CONFIG_ARCH_CHIP_STM32F302VB is not set
+# CONFIG_ARCH_CHIP_STM32F302VC is not set
+# CONFIG_ARCH_CHIP_STM32F303CB is not set
+# CONFIG_ARCH_CHIP_STM32F303CC is not set
+# CONFIG_ARCH_CHIP_STM32F303RB is not set
+# CONFIG_ARCH_CHIP_STM32F303RC is not set
+# CONFIG_ARCH_CHIP_STM32F303VB is not set
+# CONFIG_ARCH_CHIP_STM32F303VC is not set
+# CONFIG_ARCH_CHIP_STM32F405RG is not set
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+# CONFIG_ARCH_CHIP_STM32F407VG is not set
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+CONFIG_ARCH_CHIP_STM32F427V=y
+# CONFIG_ARCH_CHIP_STM32F427Z is not set
+# CONFIG_ARCH_CHIP_STM32F427I is not set
+# CONFIG_STM32_STM32L15XX is not set
+# CONFIG_STM32_ENERGYLITE is not set
+# CONFIG_STM32_STM32F10XX is not set
+# CONFIG_STM32_VALUELINE is not set
+# CONFIG_STM32_CONNECTIVITYLINE is not set
+# CONFIG_STM32_PERFORMANCELINE is not set
+# CONFIG_STM32_HIGHDENSITY is not set
+# CONFIG_STM32_MEDIUMDENSITY is not set
+# CONFIG_STM32_LOWDENSITY is not set
+# CONFIG_STM32_STM32F20XX is not set
+# CONFIG_STM32_STM32F30XX is not set
+CONFIG_STM32_STM32F40XX=y
+CONFIG_STM32_STM32F427=y
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+CONFIG_STM32_ADC1=y
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+CONFIG_STM32_BKPSRAM=y
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+CONFIG_STM32_CCMDATARAM=y
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+# CONFIG_STM32_I2C1 is not set
+CONFIG_STM32_I2C2=y
+# CONFIG_STM32_I2C3 is not set
+# CONFIG_STM32_OTGFS is not set
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+# CONFIG_STM32_SDIO is not set
+CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI2=y
+CONFIG_STM32_SPI3=y
+CONFIG_STM32_SPI4=y
+# CONFIG_STM32_SPI5 is not set
+# CONFIG_STM32_SPI6 is not set
+CONFIG_STM32_SYSCFG=y
+CONFIG_STM32_TIM1=y
+# CONFIG_STM32_TIM2 is not set
+CONFIG_STM32_TIM3=y
+CONFIG_STM32_TIM4=y
+CONFIG_STM32_TIM5=y
+# CONFIG_STM32_TIM6 is not set
+# CONFIG_STM32_TIM7 is not set
+# CONFIG_STM32_TIM8 is not set
+CONFIG_STM32_TIM9=y
+# CONFIG_STM32_TIM10 is not set
+# CONFIG_STM32_TIM11 is not set
+# CONFIG_STM32_TIM12 is not set
+# CONFIG_STM32_TIM13 is not set
+# CONFIG_STM32_TIM14 is not set
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+# CONFIG_STM32_UART4 is not set
+# CONFIG_STM32_UART5 is not set
+# CONFIG_STM32_USART6 is not set
+CONFIG_STM32_UART7=y
+CONFIG_STM32_UART8=y
+# CONFIG_STM32_IWDG is not set
+CONFIG_STM32_WWDG=y
+CONFIG_STM32_ADC=y
+CONFIG_STM32_SPI=y
+CONFIG_STM32_I2C=y
+
+#
+# Alternate Pin Mapping
+#
+CONFIG_STM32_FLASH_PREFETCH=y
+# CONFIG_STM32_JTAG_DISABLE is not set
+# CONFIG_STM32_JTAG_FULL_ENABLE is not set
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+# CONFIG_STM32_FORCEPOWER is not set
+# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
+# CONFIG_STM32_CCMEXCLUDE is not set
+CONFIG_STM32_DMACAPABLE=y
+# CONFIG_STM32_TIM1_PWM is not set
+# CONFIG_STM32_TIM3_PWM is not set
+# CONFIG_STM32_TIM4_PWM is not set
+# CONFIG_STM32_TIM5_PWM is not set
+# CONFIG_STM32_TIM9_PWM is not set
+# CONFIG_STM32_TIM1_ADC is not set
+# CONFIG_STM32_TIM3_ADC is not set
+# CONFIG_STM32_TIM4_ADC is not set
+# CONFIG_STM32_TIM5_ADC is not set
+CONFIG_STM32_USART=y
+
+#
+# U[S]ART Configuration
+#
+# CONFIG_USART1_RS485 is not set
+CONFIG_USART1_RXDMA=y
+# CONFIG_USART2_RS485 is not set
+CONFIG_USART2_RXDMA=y
+# CONFIG_USART3_RS485 is not set
+CONFIG_USART3_RXDMA=y
+# CONFIG_UART7_RS485 is not set
+CONFIG_UART7_RXDMA=y
+# CONFIG_UART8_RS485 is not set
+CONFIG_UART8_RXDMA=y
+CONFIG_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+
+#
+# SPI Configuration
+#
+# CONFIG_STM32_SPI_INTERRUPTS is not set
+# CONFIG_STM32_SPI_DMA is not set
+
+#
+# I2C Configuration
+#
+# CONFIG_STM32_I2C_DYNTIMEO is not set
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=10
+CONFIG_STM32_I2CTIMEOTICKS=500
+# CONFIG_STM32_I2C_DUTY16_9 is not set
+
+#
+# USB Host Configuration
+#
+
+#
+# USB Device Configuration
+#
+
+#
+# External Memory Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_VECNOTIRQ is not set
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_IRQPRIO=y
+# CONFIG_CUSTOM_STACK is not set
+# CONFIG_ADDRENV is not set
+CONFIG_ARCH_HAVE_VFORK=y
+CONFIG_ARCH_STACKDUMP=y
+# CONFIG_ENDIAN_BIG is not set
+# CONFIG_ARCH_HAVE_RAMFUNCS is not set
+CONFIG_ARCH_HAVE_RAMVECTORS=y
+# CONFIG_ARCH_RAMVECTORS is not set
+
+#
+# Board Settings
+#
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=262144
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=4096
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD=""
+
+#
+# Common Board Options
+#
+CONFIG_NSH_MMCSDMINOR=0
+
+#
+# Board-Specific Options
+#
+
+#
+# RTOS Features
+#
+# CONFIG_BOARD_INITIALIZE is not set
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+# CONFIG_SCHED_HAVE_PARENT is not set
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_DEV_CONSOLE=y
+# CONFIG_MUTEX_TYPES is not set
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=8
+# CONFIG_FDCLONE_DISABLE is not set
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WAITPID=y
+# CONFIG_SCHED_STARTHOOK is not set
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_ATEXIT_MAX=1
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="nsh_main"
+# CONFIG_DISABLE_OS_API is not set
+
+#
+# Signal Numbers
+#
+CONFIG_SIG_SIGUSR1=1
+CONFIG_SIG_SIGUSR2=2
+CONFIG_SIG_SIGALARM=3
+CONFIG_SIG_SIGCONDTIMEDOUT=16
+CONFIG_SIG_SIGWORK=4
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=10
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=36
+CONFIG_NFILE_STREAMS=8
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Stack and heap information
+#
+CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+# CONFIG_DISABLE_POLL is not set
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+CONFIG_I2C=y
+# CONFIG_I2C_SLAVE is not set
+CONFIG_I2C_TRANSFER=y
+# CONFIG_I2C_WRITEREAD is not set
+# CONFIG_I2C_POLLED is not set
+# CONFIG_I2C_TRACE is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+CONFIG_I2C_RESET=y
+CONFIG_SPI=y
+# CONFIG_SPI_OWNBUS is not set
+CONFIG_SPI_EXCHANGE=y
+# CONFIG_SPI_CMDDATA is not set
+# CONFIG_RTC is not set
+CONFIG_WATCHDOG=y
+# CONFIG_ANALOG is not set
+# CONFIG_AUDIO_DEVICES is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+# CONFIG_MMCSD is not set
+CONFIG_MTD=y
+
+#
+# MTD Configuration
+#
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_BYTE_WRITE=y
+
+#
+# MTD Device Drivers
+#
+# CONFIG_RAMMTD is not set
+# CONFIG_MTD_AT24XX is not set
+# CONFIG_MTD_AT45DB is not set
+# CONFIG_MTD_M25P is not set
+# CONFIG_MTD_SMART is not set
+CONFIG_MTD_RAMTRON=y
+CONFIG_RAMTRON_FUJITSU=y
+# CONFIG_MTD_SST25 is not set
+# CONFIG_MTD_SST39FV is not set
+# CONFIG_MTD_W25 is not set
+CONFIG_PIPES=y
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+CONFIG_SERIAL=y
+# CONFIG_DEV_LOWCONSOLE is not set
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_UART7=y
+CONFIG_ARCH_HAVE_UART8=y
+CONFIG_ARCH_HAVE_USART1=y
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART3=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_USART1_SERIAL_CONSOLE is not set
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+# CONFIG_USART3_SERIAL_CONSOLE is not set
+# CONFIG_UART7_SERIAL_CONSOLE is not set
+CONFIG_UART8_SERIAL_CONSOLE=y
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# USART1 Configuration
+#
+CONFIG_USART1_RXBUFSIZE=512
+CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART1_BAUD=115200
+CONFIG_USART1_BITS=8
+CONFIG_USART1_PARITY=0
+CONFIG_USART1_2STOP=0
+# CONFIG_USART1_IFLOWCONTROL is not set
+# CONFIG_USART1_OFLOWCONTROL is not set
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=512
+CONFIG_USART2_TXBUFSIZE=512
+CONFIG_USART2_BAUD=115200
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+# CONFIG_USART2_IFLOWCONTROL is not set
+# CONFIG_USART2_OFLOWCONTROL is not set
+
+#
+# USART3 Configuration
+#
+CONFIG_USART3_RXBUFSIZE=512
+CONFIG_USART3_TXBUFSIZE=512
+CONFIG_USART3_BAUD=115200
+CONFIG_USART3_BITS=8
+CONFIG_USART3_PARITY=0
+CONFIG_USART3_2STOP=0
+# CONFIG_USART3_IFLOWCONTROL is not set
+# CONFIG_USART3_OFLOWCONTROL is not set
+
+#
+# UART7 Configuration
+#
+CONFIG_UART7_RXBUFSIZE=512
+CONFIG_UART7_TXBUFSIZE=512
+CONFIG_UART7_BAUD=115200
+CONFIG_UART7_BITS=8
+CONFIG_UART7_PARITY=0
+CONFIG_UART7_2STOP=0
+# CONFIG_UART7_IFLOWCONTROL is not set
+# CONFIG_UART7_OFLOWCONTROL is not set
+
+#
+# UART8 Configuration
+#
+CONFIG_UART8_RXBUFSIZE=512
+CONFIG_UART8_TXBUFSIZE=512
+CONFIG_UART8_BAUD=115200
+CONFIG_UART8_BITS=8
+CONFIG_UART8_PARITY=0
+CONFIG_UART8_2STOP=0
+# CONFIG_UART8_IFLOWCONTROL is not set
+# CONFIG_UART8_OFLOWCONTROL is not set
+# CONFIG_SERIAL_IFLOWCONTROL is not set
+# CONFIG_SERIAL_OFLOWCONTROL is not set
+# CONFIG_USBDEV is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_DISABLE_MOUNTPOINT is not set
+# CONFIG_FS_RAMMAP is not set
+CONFIG_FS_FAT=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_MAXFNAME=32
+CONFIG_FS_FATTIME=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FS_NXFFS=y
+CONFIG_NXFFS_PREALLOCATED=y
+CONFIG_NXFFS_ERASEDSTATE=0xff
+CONFIG_NXFFS_PACKTHRESHOLD=32
+CONFIG_NXFFS_MAXNAMLEN=32
+CONFIG_NXFFS_TAILTHRESHOLD=2048
+CONFIG_FS_ROMFS=y
+# CONFIG_FS_SMARTFS is not set
+CONFIG_FS_BINFS=y
+
+#
+# System Logging
+#
+# CONFIG_SYSLOG_ENABLE is not set
+# CONFIG_SYSLOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_MULTIHEAP is not set
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+CONFIG_GRAN=y
+# CONFIG_GRAN_SINGLE is not set
+# CONFIG_GRAN_INTR is not set
+
+#
+# Audio Support
+#
+# CONFIG_AUDIO is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_BINFMT_EXEPATH is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_BUILTIN=y
+# CONFIG_PIC is not set
+# CONFIG_SYMTAB_ORDEREDBYNAME is not set
+
+#
+# Library Routines
+#
+
+#
+# Standard C Library Options
+#
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_LIB_HOMEDIR="/"
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIB_RAND_ORDER=1
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_EXECFUNCS is not set
+CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
+CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
+CONFIG_LIBC_STRERROR=y
+# CONFIG_LIBC_STRERROR_SHORT is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
+CONFIG_ARCH_MEMCPY=y
+# CONFIG_ARCH_MEMCMP is not set
+# CONFIG_ARCH_MEMMOVE is not set
+# CONFIG_ARCH_MEMSET is not set
+# CONFIG_MEMSET_OPTSPEED is not set
+# CONFIG_ARCH_STRCHR is not set
+# CONFIG_ARCH_STRCMP is not set
+# CONFIG_ARCH_STRCPY is not set
+# CONFIG_ARCH_STRNCPY is not set
+# CONFIG_ARCH_STRLEN is not set
+# CONFIG_ARCH_STRNLEN is not set
+# CONFIG_ARCH_BZERO is not set
+
+#
+# Non-standard Library Support
+#
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=2000
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=2000
+# CONFIG_LIB_KBDCODEC is not set
+# CONFIG_LIB_SLCDCODEC is not set
+
+#
+# Basic CXX Support
+#
+CONFIG_C99_BOOL8=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+# CONFIG_CXX_NEWLONG is not set
+
+#
+# uClibc++ Standard C++ Library
+#
+# CONFIG_UCLIBCXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Built-In Applications
+#
+CONFIG_BUILTIN_PROXY_STACKSIZE=1024
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_CXXTEST is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+CONFIG_EXAMPLES_HELLO=y
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_KEYPADTEST is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+CONFIG_EXAMPLES_MOUNT=y
+# CONFIG_EXAMPLES_MTDPART is not set
+# CONFIG_EXAMPLES_NRF24L01TERM is not set
+CONFIG_EXAMPLES_NSH=y
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+# CONFIG_EXAMPLES_OSTEST is not set
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POSIXSPAWN is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_SLCD is not set
+# CONFIG_EXAMPLES_SMART_TEST is not set
+# CONFIG_EXAMPLES_SMART is not set
+# CONFIG_EXAMPLES_TCPECHO is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_TIFF is not set
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# FreeModBus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+CONFIG_NSH_LIBRARY=y
+CONFIG_NSH_BUILTIN_APPS=y
+
+#
+# Disable Individual commands
+#
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DD is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_HEXDUMP is not set
+# CONFIG_NSH_DISABLE_IFCONFIG is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOSETUP is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MB is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MKFIFO is not set
+# CONFIG_NSH_DISABLE_MKRD is not set
+# CONFIG_NSH_DISABLE_MH is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MW is not set
+# CONFIG_NSH_DISABLE_NSFMOUNT is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PING is not set
+# CONFIG_NSH_DISABLE_PUT is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SH is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+# CONFIG_NSH_DISABLE_WGET is not set
+# CONFIG_NSH_DISABLE_XD is not set
+
+#
+# Configure Command Options
+#
+# CONFIG_NSH_CMDOPT_DF_H is not set
+CONFIG_NSH_CODECS_BUFSIZE=128
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLEBG is not set
+CONFIG_NSH_ROMFSETC=y
+# CONFIG_NSH_ROMFSRC is not set
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_FATDEVNO=1
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT="/tmp"
+CONFIG_NSH_CONSOLE=y
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_CONDEV is not set
+CONFIG_NSH_ARCHINIT=y
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+CONFIG_SYSTEM_I2CTOOL=y
+CONFIG_I2CTOOL_MINBUS=0
+CONFIG_I2CTOOL_MAXBUS=3
+CONFIG_I2CTOOL_MINADDR=0x03
+CONFIG_I2CTOOL_MAXADDR=0x77
+CONFIG_I2CTOOL_MAXREGADDR=0xff
+CONFIG_I2CTOOL_DEFFREQ=4000000
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+# CONFIG_SYSTEM_FLASH_ERASEALL is not set
+
+#
+# readline()
+#
+CONFIG_SYSTEM_READLINE=y
+CONFIG_READLINE_ECHO=y
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+CONFIG_SYSTEM_SYSINFO=y
+
+#
+# USB Monitor
+#
diff --git a/src/modules/position_estimator/module.mk b/nuttx-configs/aerocore/nsh/setenv.sh
index f64095d9d..2327f38a1 100644..100755
--- a/src/modules/position_estimator/module.mk
+++ b/nuttx-configs/aerocore/nsh/setenv.sh
@@ -1,6 +1,8 @@
-############################################################################
+#!/bin/bash
+# configs/aerocore/nsh/setenv.sh
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -12,7 +14,7 @@
 #    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
+# 3. Neither the name NuttX nor the names of its contributors may be
 #    used to endorse or promote products derived from this software
 #    without specific prior written permission.
 #
@@ -29,16 +31,37 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 #
-############################################################################
 
-#
-# Makefile to build the position estimator
-#
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
 
-MODULE_COMMAND		= position_estimator
+# This the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
 
-# XXX this should be converted to a deamon, its a pretty bad example app
-MODULE_PRIORITY	 	= SCHED_PRIORITY_DEFAULT
-MODULE_STACKSIZE	= 4096
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
 
-SRCS		 	= position_estimator_main.c
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/aerocore/scripts/ld.script b/nuttx-configs/aerocore/scripts/ld.script
new file mode 100644
index 000000000..968d3127f
--- /dev/null
+++ b/nuttx-configs/aerocore/scripts/ld.script
@@ -0,0 +1,150 @@
+/****************************************************************************
+ * configs/aerocore/common/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+    ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)        
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)        
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+		/* 
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/Tools/px4params/px_process_params.py b/nuttx-configs/aerocore/src/Makefile
index cdf5aba7c..859ba4ab2 100755..100644
--- a/Tools/px4params/px_process_params.py
+++ b/nuttx-configs/aerocore/src/Makefile
@@ -1,7 +1,8 @@
-#!/usr/bin/env python
 ############################################################################
+# configs/aerocore/src/Makefile
 #
-#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -13,7 +14,7 @@
 #    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
+# 3. Neither the name NuttX nor the names of its contributors may be
 #    used to endorse or promote products derived from this software
 #    without specific prior written permission.
 #
@@ -32,30 +33,52 @@
 #
 ############################################################################
 
-#
-# PX4 paramers processor (main executable file)
-#
-# It scans src/ subdirectory of the project, collects all parameters
-# definitions, and outputs list of parameters in XML and DokuWiki formats.
-#
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		= 
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	$(Q) touch $@
 
-import scanner
-import parser
-import xmlout
-import dokuwikiout
+depend: .depend
 
-# Initialize parser
-prs = parser.Parser()
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
 
-# Scan directories, and parse the files
-sc = scanner.Scanner()
-sc.ScanDir("../../src", prs)
-output = prs.GetParamGroups()
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
 
-# Output into XML
-out = xmlout.XMLOutput()
-out.Save(output, "parameters.xml")
+-include Make.dep
 
-# Output into DokuWiki
-out = dokuwikiout.DokuWikiOutput()
-out.Save(output, "parameters.wiki")
diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/aerocore/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so 
+ * we have this empty source file to keep it company.
+ */
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 1dc96b3c3..a651faffa 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_DESCRIPTORS=38
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
@@ -417,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=6000
-CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_IDLETHREAD_STACKSIZE=3500
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
@@ -504,8 +504,8 @@ CONFIG_MTD_BYTE_WRITE=y
 #
 # USART1 Configuration
 #
-CONFIG_USART1_RXBUFSIZE=512
-CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART1_RXBUFSIZE=300
+CONFIG_USART1_TXBUFSIZE=300
 CONFIG_USART1_BAUD=57600
 CONFIG_USART1_BITS=8
 CONFIG_USART1_PARITY=0
@@ -528,8 +528,8 @@ CONFIG_USART2_OFLOWCONTROL=y
 #
 # UART5 Configuration
 #
-CONFIG_UART5_RXBUFSIZE=512
-CONFIG_UART5_TXBUFSIZE=512
+CONFIG_UART5_RXBUFSIZE=300
+CONFIG_UART5_TXBUFSIZE=300
 CONFIG_UART5_BAUD=57600
 CONFIG_UART5_BITS=8
 CONFIG_UART5_PARITY=0
@@ -540,8 +540,8 @@ CONFIG_UART5_2STOP=0
 #
 # USART6 Configuration
 #
-CONFIG_USART6_RXBUFSIZE=512
-CONFIG_USART6_TXBUFSIZE=512
+CONFIG_USART6_RXBUFSIZE=128
+CONFIG_USART6_TXBUFSIZE=64
 CONFIG_USART6_BAUD=57600
 CONFIG_USART6_BITS=8
 CONFIG_USART6_PARITY=0
@@ -679,7 +679,7 @@ CONFIG_BUILTIN=y
 #
 # Standard C Library Options
 #
-CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STDIO_BUFFER_SIZE=180
 CONFIG_STDIO_LINEBUFFER=y
 CONFIG_NUNGET_CHARS=2
 CONFIG_LIB_HOMEDIR="/"
@@ -720,11 +720,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=4000
+CONFIG_SCHED_WORKSTACKSIZE=2048
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=4000
+CONFIG_SCHED_LPWORKSTACKSIZE=2048
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 
diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h
index 3bede8a1f..3b3c6fa70 100755
--- a/nuttx-configs/px4fmu-v2/include/board.h
+++ b/nuttx-configs/px4fmu-v2/include/board.h
@@ -268,6 +268,10 @@
 #define GPIO_SPI2_MOSI	(GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
 #define GPIO_SPI2_SCK	(GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
 
+#define GPIO_SPI4_MISO	(GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI4_MOSI	(GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI4_SCK	(GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz)
+
 /************************************************************************************
  * Public Data
  ************************************************************************************/
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 2a734c27e..a55c95a29 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -235,7 +235,7 @@ CONFIG_STM32_SDIO=y
 CONFIG_STM32_SPI1=y
 CONFIG_STM32_SPI2=y
 # CONFIG_STM32_SPI3 is not set
-# CONFIG_STM32_SPI4 is not set
+CONFIG_STM32_SPI4=y
 # CONFIG_STM32_SPI5 is not set
 # CONFIG_STM32_SPI6 is not set
 CONFIG_STM32_SYSCFG=y
@@ -306,7 +306,7 @@ CONFIG_UART5_RXDMA=y
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_UART7_RS485 is not set
-# CONFIG_UART7_RXDMA is not set
+CONFIG_UART7_RXDMA=y
 # CONFIG_UART8_RS485 is not set
 CONFIG_UART8_RXDMA=y
 CONFIG_SERIAL_DISABLE_REORDERING=y
@@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_DESCRIPTORS=38
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
@@ -451,8 +451,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=6000
-CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_IDLETHREAD_STACKSIZE=3500
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
@@ -539,8 +539,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2
 # CONFIG_USART3_SERIAL_CONSOLE is not set
 # CONFIG_UART4_SERIAL_CONSOLE is not set
 # CONFIG_USART6_SERIAL_CONSOLE is not set
-# CONFIG_UART7_SERIAL_CONSOLE is not set
-CONFIG_UART8_SERIAL_CONSOLE=y
+CONFIG_UART7_SERIAL_CONSOLE=y
+# CONFIG_UART8_SERIAL_CONSOLE is not set
 # CONFIG_NO_SERIAL_CONSOLE is not set
 
 #
@@ -796,11 +796,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=4000
+CONFIG_SCHED_WORKSTACKSIZE=2000
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=4000
+CONFIG_SCHED_LPWORKSTACKSIZE=2000
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 
diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h
index 4b9c90638..17e77918b 100755
--- a/nuttx-configs/px4io-v2/include/board.h
+++ b/nuttx-configs/px4io-v2/include/board.h
@@ -103,8 +103,6 @@
 #define GPIO_USART2_RTS	0xffffffff
 #undef GPIO_USART2_CK
 #define GPIO_USART2_CK 	0xffffffff
-#undef GPIO_USART3_TX
-#define GPIO_USART3_TX 	0xffffffff
 #undef GPIO_USART3_CK
 #define GPIO_USART3_CK 	0xffffffff
 #undef GPIO_USART3_CTS
diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig
index 4111e70eb..e6563e587 100755
--- a/nuttx-configs/px4io-v2/nsh/defconfig
+++ b/nuttx-configs/px4io-v2/nsh/defconfig
@@ -104,9 +104,9 @@ CONFIG_ARMV7M_CMNVECTOR=y
 #   CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
 #
 CONFIG_STM32_DFU=n
-CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_FULL_ENABLE=n
 CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
-CONFIG_STM32_JTAG_SW_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=y
 
 #
 # Individual subsystems can be enabled:
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 771f2128f..41942aacd 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -76,16 +76,18 @@
 
 #include <drivers/airspeed/airspeed.h>
 
-Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
-	I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
+Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
+	I2C("Airspeed", path, bus, address, 100000),
 	_reports(nullptr),
 	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
 	_max_differential_pressure_pa(0),
 	_sensor_ok(false),
+	_last_published_sensor_ok(true), /* initialize differently to force publication */
 	_measure_ticks(0),
 	_collect_phase(false),
 	_diff_pres_offset(0.0f),
 	_airspeed_pub(-1),
+	_subsys_pub(-1),
 	_class_instance(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
@@ -149,8 +151,7 @@ Airspeed::init()
 	}
 
 	ret = OK;
-	/* sensor is ok, but we don't really know if it is within range */
-	_sensor_ok = true;
+
 out:
 	return ret;
 }
@@ -344,22 +345,6 @@ Airspeed::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		SUBSYSTEM_TYPE_DIFFPRESSURE
-	};
-	static orb_advert_t pub = -1;
-
-	if (pub > 0) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	}
 }
 
 void
@@ -369,11 +354,34 @@ Airspeed::stop()
 }
 
 void
+Airspeed::update_status()
+{
+	if (_sensor_ok != _last_published_sensor_ok) {
+		/* notify about state change */
+		struct subsystem_info_s info = {
+			true,
+			true,
+			_sensor_ok,
+			SUBSYSTEM_TYPE_DIFFPRESSURE
+		};
+
+		if (_subsys_pub > 0) {
+			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
+		} else {
+			_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
+		}
+
+		_last_published_sensor_ok = _sensor_ok;
+	}
+}
+
+void
 Airspeed::cycle_trampoline(void *arg)
 {
 	Airspeed *dev = (Airspeed *)arg;
 
 	dev->cycle();
+	dev->update_status();
 }
 
 void
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index c27b1bcd8..0b8e949c9 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -90,7 +90,7 @@ static const int ERROR = -1;
 class __EXPORT Airspeed : public device::I2C
 {
 public:
-	Airspeed(int bus, int address, unsigned conversion_interval);
+	Airspeed(int bus, int address, unsigned conversion_interval, const char* path);
 	virtual ~Airspeed();
 
 	virtual int	init();
@@ -118,14 +118,21 @@ protected:
 	virtual int	measure() = 0;
 	virtual int	collect() = 0;
 
+	/**
+	 * Update the subsystem status
+	 */
+	void update_status();
+
 	work_s			_work;
 	float			_max_differential_pressure_pa;
 	bool			_sensor_ok;
+	bool			_last_published_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
 	float			_diff_pres_offset;
 
 	orb_advert_t		_airspeed_pub;
+	orb_advert_t		_subsys_pub;
 
 	int			_class_instance;
 
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index b88f61ce8..e5bb772b3 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -119,7 +119,7 @@ int ardrone_interface_main(int argc, char *argv[])
 		ardrone_interface_task = task_spawn_cmd("ardrone_interface",
 						    SCHED_DEFAULT,
 						    SCHED_PRIORITY_MAX - 15,
-						    2048,
+						    1100,
 						    ardrone_interface_thread_main,
 						    (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index 81f634992..fc017dd58 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -382,8 +382,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	float output_band = 0.0f;
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
 
-	static bool initialized = false;
-
 	/* linearly scale the control inputs from 0 to startpoint_full_control */
 	if (motor_thrust < startpoint_full_control) {
 		output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1
diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk
index 058bd1397..285db1351 100644
--- a/src/drivers/ardrone_interface/module.mk
+++ b/src/drivers/ardrone_interface/module.mk
@@ -38,3 +38,6 @@
 MODULE_COMMAND	 = ardrone_interface
 SRCS		 = ardrone_interface.c \
 			   ardrone_motor_control.c
+MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 2361f4dd1..6b14f5945 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -48,11 +48,14 @@
  * The recognized number off cells, will be blinked 5 times in purple color.
  * 2 Cells = 2 blinks
  * ...
- * 5 Cells = 5 blinks
+ * 6 Cells = 6 blinks
  * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
  *
- * System disarmed:
- * The BlinkM should lit solid red.
+ * System disarmed and safe:
+ * The BlinkM should light solid cyan.
+ *
+ * System safety off but not armed:
+ * The BlinkM should light flashing orange
  *
  * System armed:
  * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
@@ -67,10 +70,10 @@
  * (X = on, _=off)
  *
  * The first 3 blinks indicates the status of the GPS-Signal (red):
- * 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
- *   5 satellites = X-X-_-X-_-_-_-_-_-_-
- *   6 satellites = X-_-_-X-_-_-_-_-_-_-
- * >=7 satellites = _-_-_-X-_-_-_-_-_-_-
+ * 0-4 satellites = X-X-X-X-X-_-_-_-_-_-
+ *   5 satellites = X-X-_-X-X-_-_-_-_-_-
+ *   6 satellites = X-_-_-X-X-_-_-_-_-_-
+ * >=7 satellites = _-_-_-X-X-_-_-_-_-_-
  * If no GPS is found the first 3 blinks are white
  *
  * The fourth Blink indicates the Flightmode:
@@ -119,6 +122,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/safety.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
 static const int LED_ONTIME = 120;
@@ -166,10 +170,12 @@ private:
 	enum ledColors {
 		LED_OFF,
 		LED_RED,
+		LED_ORANGE,
 		LED_YELLOW,
 		LED_PURPLE,
 		LED_GREEN,
 		LED_BLUE,
+		LED_CYAN,
 		LED_WHITE,
 		LED_AMBER
 	};
@@ -188,6 +194,26 @@ private:
 
 	bool systemstate_run;
 
+	int vehicle_status_sub_fd;
+	int vehicle_control_mode_sub_fd;
+	int vehicle_gps_position_sub_fd;
+	int actuator_armed_sub_fd;
+	int safety_sub_fd;
+
+	int num_of_cells;
+	int detected_cells_runcount;
+
+	int t_led_color[8];
+	int t_led_blink;
+	int led_thread_runcount;
+	int led_interval;
+
+	bool topic_initialized;
+	bool detected_cells_blinked;
+	bool led_thread_ready;
+
+	int num_of_used_sats;
+
 	void 			setLEDColor(int ledcolor);
 	static void		led_trampoline(void *arg);
 	void			led();
@@ -259,7 +285,22 @@ BlinkM::BlinkM(int bus, int blinkm) :
 	led_color_7(LED_OFF),
 	led_color_8(LED_OFF),
 	led_blink(LED_NOBLINK),
-	systemstate_run(false)
+	systemstate_run(false),
+	vehicle_status_sub_fd(-1),
+	vehicle_control_mode_sub_fd(-1),
+	vehicle_gps_position_sub_fd(-1),
+	actuator_armed_sub_fd(-1),
+	safety_sub_fd(-1),
+	num_of_cells(0),
+	detected_cells_runcount(0),
+	t_led_color{0},
+	t_led_blink(0),
+	led_thread_runcount(0),
+	led_interval(1000),
+	topic_initialized(false),
+	detected_cells_blinked(false),
+	led_thread_ready(true),
+	num_of_used_sats(0)
 {
 	memset(&_work, 0, sizeof(_work));
 }
@@ -376,42 +417,22 @@ void
 BlinkM::led()
 {
 
-	static int vehicle_status_sub_fd;
-	static int vehicle_control_mode_sub_fd;
-	static int vehicle_gps_position_sub_fd;
-	static int actuator_armed_sub_fd;
-
-	static int num_of_cells = 0;
-	static int detected_cells_runcount = 0;
-
-	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
-	static int t_led_blink = 0;
-	static int led_thread_runcount=0;
-	static int led_interval = 1000;
-
-	static int no_data_vehicle_status = 0;
-	static int no_data_vehicle_control_mode = 0;
-	static int no_data_actuator_armed = 0;
-	static int no_data_vehicle_gps_position = 0;
-
-	static bool topic_initialized = false;
-	static bool detected_cells_blinked = false;
-	static bool led_thread_ready = true;
-
-	int num_of_used_sats = 0;
-
 	if(!topic_initialized) {
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
-		orb_set_interval(vehicle_status_sub_fd, 1000);
+		orb_set_interval(vehicle_status_sub_fd, 250);
 
 		vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
-		orb_set_interval(vehicle_control_mode_sub_fd, 1000);
+		orb_set_interval(vehicle_control_mode_sub_fd, 250);
 
 		actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
-		orb_set_interval(actuator_armed_sub_fd, 1000);
+		orb_set_interval(actuator_armed_sub_fd, 250);
 
 		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
-		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
+		orb_set_interval(vehicle_gps_position_sub_fd, 250);
+
+		/* Subscribe to safety topic */
+		safety_sub_fd = orb_subscribe(ORB_ID(safety));
+		orb_set_interval(safety_sub_fd, 250);
 
 		topic_initialized = true;
 	}
@@ -433,7 +454,9 @@ BlinkM::led()
 			if(num_of_cells > 4) {
 				t_led_color[4] = LED_PURPLE;
 			}
-			t_led_color[5] = LED_OFF;
+			if(num_of_cells > 5) {
+				t_led_color[5] = LED_PURPLE;
+			}
 			t_led_color[6] = LED_OFF;
 			t_led_color[7] = LED_OFF;
 			t_led_blink = LED_BLINK;
@@ -467,17 +490,25 @@ BlinkM::led()
 		struct vehicle_control_mode_s vehicle_control_mode;
 		struct actuator_armed_s actuator_armed;
 		struct vehicle_gps_position_s vehicle_gps_position_raw;
+		struct safety_s safety;
 
 		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
 		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
+		memset(&safety, 0, sizeof(safety));
 
 		bool new_data_vehicle_status;
 		bool new_data_vehicle_control_mode;
 		bool new_data_actuator_armed;
 		bool new_data_vehicle_gps_position;
+		bool new_data_safety;
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
 
+		int no_data_vehicle_status = 0;
+		int no_data_vehicle_control_mode = 0;
+		int no_data_actuator_armed = 0;
+		int no_data_vehicle_gps_position = 0;
+
 		if (new_data_vehicle_status) {
 			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
 			no_data_vehicle_status = 0;
@@ -520,16 +551,15 @@ BlinkM::led()
 				no_data_vehicle_gps_position = 3;
 		}
 
+		/* update safety topic */
+		orb_check(safety_sub_fd, &new_data_safety);
 
+		if (new_data_safety) {
+			orb_copy(ORB_ID(safety), safety_sub_fd, &safety);
+		}
 
 		/* get number of used satellites in navigation */
-		num_of_used_sats = 0;
-
-		for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
-			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
-				num_of_used_sats++;
-			}
-		}
+		num_of_used_sats = vehicle_gps_position_raw.satellites_used;
 
 		if (new_data_vehicle_status || no_data_vehicle_status < 3) {
 			if (num_of_cells == 0) {
@@ -541,19 +571,7 @@ BlinkM::led()
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
-					/* LED Pattern for battery low warning */
-					led_color_1 = LED_YELLOW;
-					led_color_2 = LED_YELLOW;
-					led_color_3 = LED_YELLOW;
-					led_color_4 = LED_YELLOW;
-					led_color_5 = LED_YELLOW;
-					led_color_6 = LED_YELLOW;
-					led_color_7 = LED_YELLOW;
-					led_color_8 = LED_YELLOW;
-					led_blink = LED_BLINK;
-
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -565,21 +583,56 @@ BlinkM::led()
 					led_color_8 = LED_RED;
 					led_blink = LED_BLINK;
 
+				} else if(vehicle_status_raw.rc_signal_lost) {
+					/* LED Pattern for FAILSAFE */
+					led_color_1 = LED_BLUE;
+					led_color_2 = LED_BLUE;
+					led_color_3 = LED_BLUE;
+					led_color_4 = LED_BLUE;
+					led_color_5 = LED_BLUE;
+					led_color_6 = LED_BLUE;
+					led_color_7 = LED_BLUE;
+					led_color_8 = LED_BLUE;
+					led_blink = LED_BLINK;
+
+				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+					/* LED Pattern for battery low warning */
+					led_color_1 = LED_YELLOW;
+					led_color_2 = LED_YELLOW;
+					led_color_3 = LED_YELLOW;
+					led_color_4 = LED_YELLOW;
+					led_color_5 = LED_YELLOW;
+					led_color_6 = LED_YELLOW;
+					led_color_7 = LED_YELLOW;
+					led_color_8 = LED_YELLOW;
+					led_blink = LED_BLINK;
+
 				} else {
 					/* no battery warnings here */
 
 					if(actuator_armed.armed == false) {
 						/* system not armed */
-						led_color_1 = LED_RED;
-						led_color_2 = LED_RED;
-						led_color_3 = LED_RED;
-						led_color_4 = LED_RED;
-						led_color_5 = LED_RED;
-						led_color_6 = LED_RED;
-						led_color_7 = LED_RED;
-						led_color_8 = LED_RED;
-						led_blink = LED_NOBLINK;
-
+						if(safety.safety_off){
+							led_color_1 = LED_ORANGE;
+							led_color_2 = LED_ORANGE;
+							led_color_3 = LED_ORANGE;
+							led_color_4 = LED_ORANGE;
+							led_color_5 = LED_ORANGE;
+							led_color_6 = LED_ORANGE;
+							led_color_7 = LED_ORANGE;
+							led_color_8 = LED_ORANGE;
+							led_blink = LED_BLINK;
+						}else{
+							led_color_1 = LED_CYAN;
+							led_color_2 = LED_CYAN;
+							led_color_3 = LED_CYAN;
+							led_color_4 = LED_CYAN;
+							led_color_5 = LED_CYAN;
+							led_color_6 = LED_CYAN;
+							led_color_7 = LED_CYAN;
+							led_color_8 = LED_CYAN;
+							led_blink = LED_NOBLINK;
+						}
 					} else {
 						/* armed system - initial led pattern */
 						led_color_1 = LED_RED;
@@ -593,23 +646,23 @@ BlinkM::led()
 						led_blink = LED_BLINK;
 
 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
-
-							//XXX please check
-							if (vehicle_control_mode.flag_control_position_enabled)
+							/* indicate main control state */
+							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
-							else if (vehicle_control_mode.flag_control_velocity_enabled)
+							/* TODO: add other Auto modes */
+							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_control_mode.flag_control_attitude_enabled)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
-							else if (vehicle_control_mode.flag_control_manual_enabled)
-								led_color_4 = LED_AMBER;
-							else 
+							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
+								led_color_4 = LED_WHITE;
+							else
 								led_color_4 = LED_OFF;
-								
+							led_color_5 = led_color_4;
 						}
 
 						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
-							/* handling used sat�s */
+							/* handling used satus */
 							if(num_of_used_sats >= 7) {
 								led_color_1 = LED_OFF;
 								led_color_2 = LED_OFF;
@@ -690,8 +743,11 @@ void BlinkM::setLEDColor(int ledcolor) {
 		case LED_RED:	// red
 			set_rgb(255,0,0);
 			break;
+		case LED_ORANGE:	// orange
+			set_rgb(255,150,0);
+			break;
 		case LED_YELLOW:	// yellow
-			set_rgb(255,70,0);
+			set_rgb(200,200,0);
 			break;
 		case LED_PURPLE:	// purple
 			set_rgb(255,0,255);
@@ -702,11 +758,14 @@ void BlinkM::setLEDColor(int ledcolor) {
 		case LED_BLUE:	// blue
 			set_rgb(0,0,255);
 			break;
+		case LED_CYAN:	// cyan
+			set_rgb(0,128,128);
+			break;
 		case LED_WHITE:	// white
 			set_rgb(255,255,255);
 			break;
 		case LED_AMBER:	// amber
-			set_rgb(255,20,0);
+			set_rgb(255,65,0);
 			break;
 	}
 }
diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk
index b48b90f3f..c90673317 100644
--- a/src/drivers/blinkm/module.mk
+++ b/src/drivers/blinkm/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= blinkm
 
 SRCS		= blinkm.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c
new file mode 100644
index 000000000..4e3ba2d7e
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_init.c
@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 aerocore_init.c
+ *
+ * AeroCore-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/* 
+ * The DMA heap size constrains the total number of things that can be 
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file. 
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi3;
+static struct spi_dev_s *spi4;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	stm32_configgpio(GPIO_ADC1_IN11);	/* J1 breakout */
+	stm32_configgpio(GPIO_ADC1_IN12);	/* J1 breakout */
+	stm32_configgpio(GPIO_ADC1_IN13);	/* J1 breakout */
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+
+	/* Configure Sensors on SPI bus #3 */
+	spi3 = up_spiinitialize(3);
+	if (!spi3) {
+		message("[boot] FAILED to initialize SPI port 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+	/* Default: 1MHz, 8 bits, Mode 3 */
+	SPI_SETFREQUENCY(spi3, 10000000);
+	SPI_SETBITS(spi3, 8);
+	SPI_SETMODE(spi3, SPIDEV_MODE3);
+	SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false);
+	SPI_SELECT(spi3, PX4_SPIDEV_BARO, false);
+	up_udelay(20);
+	message("[boot] Initialized SPI port 3 (SENSORS)\n");
+
+	/* Configure FRAM on SPI bus #4 */
+	spi4 = up_spiinitialize(4);
+	if (!spi4) {
+		message("[boot] FAILED to initialize SPI port 4\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+	/* Default: ~10MHz, 8 bits, Mode 3 */
+	SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000);
+	SPI_SETBITS(spi4, 8);
+	SPI_SETMODE(spi4, SPIDEV_MODE0);
+	SPI_SELECT(spi4, SPIDEV_FLASH, false);
+	message("[boot] Initialized SPI port 4 (FRAM)\n");
+
+	return OK;
+}
diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c
new file mode 100644
index 000000000..e40d1730c
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_led.c
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   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 aerocore_led.c
+ *
+ * AeroCore LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	stm32_configgpio(GPIO_LED0);
+	stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+	switch (led) {
+	case 0:
+		stm32_gpiowrite(GPIO_LED0, true);
+		break;
+
+	case 1:
+		stm32_gpiowrite(GPIO_LED1, true);
+		break;
+
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	switch (led) {
+	case 0:
+		stm32_gpiowrite(GPIO_LED0, false);
+		break;
+
+	case 1:
+		stm32_gpiowrite(GPIO_LED1, false);
+		break;
+
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	switch (led) {
+	case 0:
+		if (stm32_gpioread(GPIO_LED0))
+			stm32_gpiowrite(GPIO_LED0, false);
+		else
+			stm32_gpiowrite(GPIO_LED0, true);
+		break;
+
+	case 1:
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+		break;
+
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
diff --git a/src/drivers/boards/aerocore/aerocore_pwm_servo.c b/src/drivers/boards/aerocore/aerocore_pwm_servo.c
new file mode 100644
index 000000000..251eaff7b
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_pwm_servo.c
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 aerocore_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM1_BASE,
+		.clock_register = STM32_RCC_APB2ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1500,
+	}
+};
diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c
new file mode 100644
index 000000000..e329bd9d1
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_spi.c
@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 aerocore_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI1_NSS);
+	stm32_gpiowrite(GPIO_SPI1_NSS, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI2_NSS);
+	stm32_gpiowrite(GPIO_SPI2_NSS, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI3
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+
+	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
+	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
+	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
+#endif
+
+#ifdef CONFIG_STM32_SPI4
+	stm32_configgpio(GPIO_SPI4_NSS);
+	stm32_gpiowrite(GPIO_SPI4_NSS, 1);
+#endif
+}
+
+#ifdef CONFIG_STM32_SPI1
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there is only one device broken-out so select it */
+	stm32_gpiowrite(GPIO_SPI1_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there is only one device broken-out so select it */
+	stm32_gpiowrite(GPIO_SPI2_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_STM32_SPI3
+__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+
+#ifdef CONFIG_STM32_SPI4
+__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI4_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h
new file mode 100644
index 000000000..70142a314
--- /dev/null
+++ b/src/drivers/boards/aerocore/board_config.h
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   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 board_config.h
+ *
+ * AeroCore internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+#define UDID_START		0x1FFF7A10
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+
+/* LEDs */
+#define GPIO_LED0	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_LED1	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10)
+
+/* Gyro */
+#define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0)
+#define SENSOR_BOARD_ROTATION_DEFAULT	3 /* SENSOR_BOARD_ROTATION_270_DEG */
+
+/* Accel & Mag */
+#define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2)
+
+/* GPS */
+#define GPIO_GPS_NRESET		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPS_TIMEPULSE	(GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4)
+#define GPS_DEFAULT_UART_PORT	"/dev/ttyS0"
+
+/* SPI3--Sensors */
+#define PX4_SPI_BUS_SENSORS	3
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2)
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
+
+/* Nominal chip selects for devices on SPI bus #3 */
+#define PX4_SPIDEV_ACCEL_MAG	0
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_BARO		2
+
+/* User GPIOs broken out on J11 */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO8_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO9_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GPIO10_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_GPIO11_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8)
+
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO8_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO9_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GPIO10_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_GPIO11_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+
+/* PWM
+ *
+ * Eight PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PA8  : TIM1_CH1
+ * CH2 : PA9  : TIM1_CH2
+ * CH3 : PA10 : TIM1_CH3
+ * CH4 : PA11 : TIM1_CH4
+ * CH5 : PC6  : TIM3_CH1
+ * CH6 : PC7  : TIM3_CH2
+ * CH7 : PC8  : TIM3_CH3
+ * CH8 : PC9  : TIM3_CH4
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_1
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_1
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_1
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_1
+#define GPIO_TIM3_CH1OUT	GPIO_TIM3_CH1OUT_3
+#define GPIO_TIM3_CH2OUT	GPIO_TIM3_CH2OUT_3
+#define GPIO_TIM3_CH3OUT	GPIO_TIM3_CH3OUT_2
+#define GPIO_TIM3_CH4OUT	GPIO_TIM3_CH4OUT_2
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer 8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/* Tone Alarm (no onboard speaker )*/
+#define TONE_ALARM_TIMER	2	/* timer 2 */
+#define TONE_ALARM_CHANNEL	1	/* channel 1 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0)
+
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk
new file mode 100644
index 000000000..b53fe0a29
--- /dev/null
+++ b/src/drivers/boards/aerocore/module.mk
@@ -0,0 +1,8 @@
+#
+# Board-specific startup code for the AeroCore
+#
+
+SRCS		 = aerocore_init.c \
+		   aerocore_pwm_servo.c \
+		   aerocore_spi.c \
+		   aerocore_led.c
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 02c26b5c0..a70a6240c 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -85,6 +85,8 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
 
+#define PX4_SPI_BUS_SENSORS	1
+
 /*
  * Use these in place of the spi_dev_e enumeration to
  * select a specific SPI device on SPI1
@@ -96,7 +98,7 @@ __BEGIN_DECLS
 /*
  * Optional devices on IO's external port
  */
-#define PX4_SPIDEV_ACCEL_MAG 2
+#define PX4_SPIDEV_ACCEL_MAG	2
 
 /*
  * I2C busses
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 4b12b75f9..293021f8b 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -99,7 +99,7 @@
  * CONFIG_ARCH_LEDS configuration switch.
  */
 __BEGIN_DECLS
-extern void led_init();
+extern void led_init(void);
 extern void led_on(int led);
 extern void led_off(int led);
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
index ea91f34ad..ee53fc43d 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -54,13 +54,13 @@
  * CONFIG_ARCH_LEDS configuration switch.
  */
 __BEGIN_DECLS
-extern void led_init();
+extern void led_init(void);
 extern void led_on(int led);
 extern void led_off(int led);
 extern void led_toggle(int led);
 __END_DECLS
 
-__EXPORT void led_init()
+__EXPORT void led_init(void)
 {
 	/* Configure LED1-2 GPIOs for output */
 
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 7cfca7656..0190a5b5b 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -106,6 +106,13 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
 #define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
+#define GPIO_SPI_CS_EXT0	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
+#define GPIO_SPI_CS_EXT1	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SPI_CS_EXT2	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_EXT3	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+
+#define PX4_SPI_BUS_SENSORS	1
+#define PX4_SPI_BUS_EXT		4
 
 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
 #define PX4_SPIDEV_GYRO		1
@@ -113,9 +120,22 @@ __BEGIN_DECLS
 #define PX4_SPIDEV_BARO		3
 #define PX4_SPIDEV_MPU		4
 
+/* External bus */
+#define PX4_SPIDEV_EXT0		1
+#define PX4_SPIDEV_EXT1		2
+#define PX4_SPIDEV_EXT2		3
+#define PX4_SPIDEV_EXT3		4
+
+/* FMUv3 SPI on external bus */
+#define PX4_SPIDEV_EXT_MPU		PX4_SPIDEV_EXT0
+#define PX4_SPIDEV_EXT_BARO		PX4_SPIDEV_EXT1
+#define PX4_SPIDEV_EXT_ACCEL_MAG	PX4_SPIDEV_EXT2
+#define PX4_SPIDEV_EXT_GYRO		PX4_SPIDEV_EXT3
+
 /* I2C busses */
 #define PX4_I2C_BUS_EXPANSION	1
-#define PX4_I2C_BUS_LED		2
+#define PX4_I2C_BUS_ONBOARD	2
+#define PX4_I2C_BUS_LED		PX4_I2C_BUS_ONBOARD
 
 /* Devices on the onboard bus.
  *
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 71414d62c..bf41bb1fe 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -192,6 +192,7 @@ stm32_boardinitialize(void)
 
 static struct spi_dev_s *spi1;
 static struct spi_dev_s *spi2;
+static struct spi_dev_s *spi4;
 static struct sdio_dev_s *sdio;
 
 #include <math.h>
@@ -305,6 +306,17 @@ __EXPORT int nsh_archinitialize(void)
 
 	message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
 
+	spi4 = up_spiinitialize(4);
+
+	/* Default SPI4 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi4, 10000000);
+	SPI_SETBITS(spi4, 8);
+	SPI_SETMODE(spi4, SPIDEV_MODE3);
+	SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
+	SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
+
+	message("[boot] Initialized SPI port 4\n");
+
 	#ifdef CONFIG_MMCSD
 	/* First, get an instance of the SDIO interface */
 
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
index a856ccb02..3c05bfa46 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
@@ -54,7 +54,7 @@
  * CONFIG_ARCH_LEDS configuration switch.
  */
 __BEGIN_DECLS
-extern void led_init();
+extern void led_init(void);
 extern void led_on(int led);
 extern void led_off(int led);
 extern void led_toggle(int led);
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
index c66c490a7..8c37d31a7 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -94,6 +94,17 @@ __EXPORT void weak_function stm32_spiinitialize(void)
 	stm32_configgpio(GPIO_SPI_CS_FRAM);
 	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
 #endif
+
+#ifdef CONFIG_STM32_SPI4
+	stm32_configgpio(GPIO_SPI_CS_EXT0);
+	stm32_configgpio(GPIO_SPI_CS_EXT1);
+	stm32_configgpio(GPIO_SPI_CS_EXT2);
+	stm32_configgpio(GPIO_SPI_CS_EXT3);
+	stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+#endif
 }
 
 __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
@@ -157,3 +168,51 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 	return SPI_STATUS_PRESENT;
 }
 #endif
+
+__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_EXT0:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		break;
+
+	case PX4_SPIDEV_EXT1:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		break;
+
+	case PX4_SPIDEV_EXT2:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		break;
+
+	case PX4_SPIDEV_EXT3:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
+		break;
+
+	default:
+		break;
+
+	}
+}
+
+__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index b157b3f18..39fb89501 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname)
 			if (ret == OK) break;
 		} else {
 			char name[32];
-			snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+			snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
 			ret = register_driver(name, &fops, 0666, (void *)this);
 			if (ret == OK) break;
 		}
@@ -268,6 +268,13 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
+	/* try the superclass. The different ioctl() function form
+         * means we need to copy arg */
+        unsigned arg2 = arg;
+	int ret = Device::ioctl(cmd, arg2);
+	if (ret != -ENODEV)
+		return ret;
+
 	return -ENOTTY;
 }
 
diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
index 683455149..f1f777dce 100644
--- a/src/drivers/device/device.cpp
+++ b/src/drivers/device/device.cpp
@@ -42,6 +42,7 @@
 #include <nuttx/arch.h>
 #include <stdio.h>
 #include <unistd.h>
+#include <drivers/drv_device.h>
 
 namespace device
 {
@@ -93,6 +94,13 @@ Device::Device(const char *name,
 	_irq_attached(false)
 {
 	sem_init(&_lock, 0, 1);
+        
+	/* setup a default device ID. When bus_type is UNKNOWN the
+	   other fields are invalid */
+	_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
+	_device_id.devid_s.bus = 0;
+	_device_id.devid_s.address = 0;
+	_device_id.devid_s.devtype = 0;
 }
 
 Device::~Device()
@@ -238,6 +246,10 @@ Device::write(unsigned offset, void *data, unsigned count)
 int
 Device::ioctl(unsigned operation, unsigned &arg)
 {
+	switch (operation) {
+	case DEVIOCGDEVICEID:
+		return (int)_device_id.devid;
+	}
 	return -ENODEV;
 }
 
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index d99f22922..7df234cab 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -124,9 +124,37 @@ public:
 	 */
 	virtual int	ioctl(unsigned operation, unsigned &arg);
 
+	/*
+	  device bus types for DEVID
+	 */
+	enum DeviceBusType {
+		DeviceBusType_UNKNOWN = 0,
+		DeviceBusType_I2C     = 1,
+		DeviceBusType_SPI     = 2
+	};
+
+	/*
+	  broken out device elements. The bitfields are used to keep
+	  the overall value small enough to fit in a float accurately,
+	  which makes it possible to transport over the MAVLink
+	  parameter protocol without loss of information.
+	 */
+	struct DeviceStructure {
+		enum DeviceBusType bus_type:3;
+		uint8_t bus:5;     // which instance of the bus type
+		uint8_t address;   // address on the bus (eg. I2C address)
+		uint8_t devtype;   // device class specific device type
+	};
+
+	union DeviceId {
+		struct DeviceStructure devid_s;
+		uint32_t devid;
+	};
+
 protected:
 	const char	*_name;			/**< driver name */
 	bool		_debug_enabled;		/**< if true, debug messages are printed */
+	union DeviceId	_device_id;             /**< device identifier information */
 
 	/**
 	 * Constructor
diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp
index a416801eb..286778c01 100644
--- a/src/drivers/device/i2c.cpp
+++ b/src/drivers/device/i2c.cpp
@@ -62,6 +62,12 @@ I2C::I2C(const char *name,
 	_frequency(frequency),
 	_dev(nullptr)
 {
+	// fill in _device_id fields for a I2C device
+	_device_id.devid_s.bus_type = DeviceBusType_I2C;
+	_device_id.devid_s.bus = bus;
+	_device_id.devid_s.address = address;
+	// devtype needs to be filled in by the driver
+	_device_id.devid_s.devtype = 0;     
 }
 
 I2C::~I2C()
@@ -201,4 +207,4 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
 	return ret;
 }
 
-} // namespace device
\ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index fed6c312c..200acac05 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -69,12 +69,18 @@ SPI::SPI(const char *name,
 	// protected
 	locking_mode(LOCK_PREEMPTION),
 	// private
-	_bus(bus),
 	_device(device),
 	_mode(mode),
 	_frequency(frequency),
-	_dev(nullptr)
+	_dev(nullptr),
+	_bus(bus)
 {
+	// fill in _device_id fields for a SPI device
+	_device_id.devid_s.bus_type = DeviceBusType_SPI;
+	_device_id.devid_s.bus = bus;
+	_device_id.devid_s.address = (uint8_t)device;
+	// devtype needs to be filled in by the driver
+	_device_id.devid_s.devtype = 0; 
 }
 
 SPI::~SPI()
@@ -133,26 +139,44 @@ SPI::probe()
 int
 SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
-	irqstate_t	state;
+	int result;
 
 	if ((send == nullptr) && (recv == nullptr))
 		return -EINVAL;
 
+	LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode;
+
 	/* lock the bus as required */
-	if (!up_interrupt_context()) {
-		switch (locking_mode) {
-		default:
-		case LOCK_PREEMPTION:
-			state = irqsave();
-			break;
-		case LOCK_THREADS:
-			SPI_LOCK(_dev, true);
-			break;
-		case LOCK_NONE:
-			break;
+	switch (mode) {
+	default:
+	case LOCK_PREEMPTION:
+		{
+			irqstate_t state = irqsave();
+			result = _transfer(send, recv, len);
+			irqrestore(state);
 		}
+		break;
+	case LOCK_THREADS:
+		SPI_LOCK(_dev, true);
+		result = _transfer(send, recv, len);
+		SPI_LOCK(_dev, false);
+		break;
+	case LOCK_NONE:
+		result = _transfer(send, recv, len);
+		break;
 	}
+	return result;
+}
 
+void 
+SPI::set_frequency(uint32_t frequency)
+{
+	_frequency = frequency;
+}
+
+int
+SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
 	SPI_SETFREQUENCY(_dev, _frequency);
 	SPI_SETMODE(_dev, _mode);
 	SPI_SETBITS(_dev, 8);
@@ -164,27 +188,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	/* and clean up */
 	SPI_SELECT(_dev, _device, false);
 
-	if (!up_interrupt_context()) {
-		switch (locking_mode) {
-		default:
-		case LOCK_PREEMPTION:
-			irqrestore(state);
-			break;
-		case LOCK_THREADS:
-			SPI_LOCK(_dev, false);
-			break;
-		case LOCK_NONE:
-			break;
-		}
-	}
-
 	return OK;
 }
 
-void 
-SPI::set_frequency(uint32_t frequency)
-{
-	_frequency = frequency;
-}
-
 } // namespace device
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index 299575dc5..54849c8c3 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -124,11 +124,15 @@ protected:
 	LockMode	locking_mode;	/**< selected locking mode */
 
 private:
-	int			_bus;
 	enum spi_dev_e		_device;
 	enum spi_mode_e		_mode;
 	uint32_t		_frequency;
 	struct spi_dev_s	*_dev;
+
+protected:
+	int			_bus;
+
+	int	_transfer(uint8_t *send, uint8_t *recv, unsigned len);
 };
 
 } // namespace device
diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h
index b310beb74..19d55cef3 100644
--- a/src/drivers/drv_device.h
+++ b/src/drivers/drv_device.h
@@ -59,4 +59,11 @@
 /** check publication block status */
 #define DEVIOCGPUBBLOCK	_DEVICEIOC(1)
 
+/**
+ * Return device ID, to enable matching of configuration parameters
+ * (such as compass offsets) to specific sensors
+ */
+#define DEVIOCGDEVICEID	_DEVICEIOC(2)
+
+
 #endif /* _DRV_DEVICE_H */
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index f60964c2b..5acd0d343 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -94,6 +94,14 @@
 
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+/*
+ * AeroCore GPIO numbers and configuration.
+ *
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 /* no GPIO driver on the PX4IOv1 board */
 #endif
@@ -146,4 +154,4 @@
 
 #define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
 
-#endif /* _DRV_GPIO_H */
\ No newline at end of file
+#endif /* _DRV_GPIO_H */
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 06e3535b3..e14f4e00d 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -43,10 +43,14 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#include "board_config.h"
+
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
+#ifndef GPS_DEFAULT_UART_PORT
 #define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
+#endif
 
 #define GPS_DEVICE_PATH	"/dev/gps"
 
diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h
new file mode 100644
index 000000000..106354377
--- /dev/null
+++ b/src/drivers/drv_io_expander.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   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 drv_io_expander.h
+ *
+ * IO expander device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/*
+ * ioctl() definitions
+ */
+
+#define _IOXIOCBASE		(0x2800)
+#define _IOXIOC(_n)		(_IOC(_IOXIOCBASE, _n))
+
+/** set a bitmask (non-blocking) */
+#define IOX_SET_MASK		_IOXIOC(1)
+
+/** get a bitmask (blocking) */
+#define IOX_GET_MASK		_IOXIOC(2)
+
+/** set device mode (non-blocking) */
+#define IOX_SET_MODE		_IOXIOC(3)
+
+/** set constant values (non-blocking) */
+#define IOX_SET_VALUE		_IOXIOC(4)
+
+/* ... to IOX_SET_VALUE + 8 */
+
+/* enum passed to RGBLED_SET_MODE ioctl()*/
+enum IOX_MODE {
+	IOX_MODE_OFF,
+	IOX_MODE_ON,
+	IOX_MODE_TEST_OUT
+};
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 06107bd3d..a259ac9c0 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -81,6 +81,13 @@ struct mag_scale {
  */
 ORB_DECLARE(sensor_mag);
 
+
+/*
+ * mag device types, for _device_id
+ */
+#define DRV_MAG_DEVTYPE_HMC5883 1
+#define DRV_MAG_DEVTYPE_LSM303D 2
+
 /*
  * ioctl() definitions
  */
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 7a93e513e..972573f9f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -199,6 +199,9 @@ ORB_DECLARE(output_pwm);
 /** get the lockdown override flag to enable outputs in HIL */
 #define PWM_SERVO_GET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 22)
 
+/** force safety switch off (to disable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
+
 /*
  *
  *
diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h
new file mode 100644
index 000000000..76ec55c3e
--- /dev/null
+++ b/src/drivers/drv_px4flow.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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 Rangefinder driver interface.
+ */
+
+#ifndef _DRV_PX4FLOW_H
+#define _DRV_PX4FLOW_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define PX4FLOW_DEVICE_PATH	"/dev/px4flow"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Optical flow in NED body frame in SI units.
+ *
+ * @see http://en.wikipedia.org/wiki/International_System_of_Units
+ *
+ * @warning If possible the usage of the raw flow and performing rotation-compensation
+ *		using the autopilot angular rate estimate is recommended.
+ */
+struct px4flow_report {
+
+	uint64_t timestamp;		/**< in microseconds since system start          */
+
+	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
+	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
+	float flow_comp_x_m;		/**< speed over ground in meters per second, rotation-compensated */
+	float flow_comp_y_m;		/**< speed over ground in meters per second, rotation-compensated */
+	float ground_distance_m;	/**< Altitude / distance to ground in meters */
+	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
+	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
+
+};
+
+/**
+ * @}
+ */
+
+/*
+ * ObjDev tag for px4flow data.
+ */
+ORB_DECLARE(optical_flow);
+
+/*
+ * ioctl() definitions
+ *
+ * px4flow drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _PX4FLOWIOCBASE			(0x7700)
+#define __PX4FLOWIOC(_n)		(_IOC(_PX4FLOWIOCBASE, _n))
+
+
+#endif /* _DRV_PX4FLOW_H */
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index cf91f7030..0f8362f58 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -46,6 +46,15 @@
 
 #define RANGE_FINDER_DEVICE_PATH	"/dev/range_finder"
 
+enum RANGE_FINDER_TYPE {
+	RANGE_FINDER_TYPE_LASER = 0,
+};
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /**
  * range finder report structure.  Reads from the device must be in multiples of this
  * structure.
@@ -53,10 +62,17 @@
 struct range_finder_report {
 	uint64_t timestamp;
 	uint64_t error_count;
-	float distance; 			/** in meters */
-	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
+	unsigned type;				/**< type, following RANGE_FINDER_TYPE enum */
+	float distance; 			/**< in meters */
+	float minimum_distance;			/**< minimum distance the sensor can measure */
+	float maximum_distance;			/**< maximum distance the sensor can measure */
+	uint8_t valid;				/**< 1 == within sensor range, 0 = outside sensor range */
 };
 
+/**
+ * @}
+ */
+
 /*
  * ObjDev tag for raw range finder data.
  */
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 20763e265..47fa8fa59 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -68,6 +68,11 @@
 #define RC_INPUT_RSSI_MAX	255
 
 /**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
  * Input signal type, value is a control position from zero to 100
  * percent.
  */
@@ -141,6 +146,10 @@ struct rc_input_values {
 	rc_input_t		values[RC_INPUT_MAX_CHANNELS];
 };
 
+/**
+ * @}
+ */
+
 /*
  * ObjDev tag for R/C inputs.
  */
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index cb5fd53a5..b7981e9c4 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -147,6 +147,8 @@ enum {
 	TONE_BATTERY_WARNING_SLOW_TUNE,
 	TONE_BATTERY_WARNING_FAST_TUNE,
 	TONE_GPS_WARNING_TUNE,
+	TONE_ARMING_FAILURE_TUNE,
+	TONE_PARACHUTE_RELEASE_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 8bbef5cfa..c15a0cee4 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -77,6 +77,7 @@
 
 /* I2C bus address */
 #define I2C_ADDRESS	0x75	/* 7-bit address. 8-bit address is 0xEA */
+#define ETS_PATH	"/dev/ets_airspeed"
 
 /* Register address */
 #define READ_CMD	0x07	/* Read the data */
@@ -93,7 +94,7 @@
 class ETSAirspeed : public Airspeed
 {
 public:
-	ETSAirspeed(int bus, int address = I2C_ADDRESS);
+	ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH);
 
 protected:
 
@@ -112,8 +113,8 @@ protected:
  */
 extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
 
-ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
-	CONVERSION_INTERVAL)
+ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
+	CONVERSION_INTERVAL, path)
 {
 
 }
@@ -131,7 +132,6 @@ ETSAirspeed::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
 	}
 
 	return ret;
@@ -154,8 +154,9 @@ ETSAirspeed::collect()
 		return ret;
 	}
 
-	uint16_t diff_pres_pa = val[1] << 8 | val[0];
-        if (diff_pres_pa == 0) {
+	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
+	uint16_t diff_pres_pa;
+        if (diff_pres_pa_raw == 0) {
 		// a zero value means the pressure sensor cannot give us a
 		// value. We need to return, and not report a value or the
 		// caller could end up using this value as part of an
@@ -165,23 +166,29 @@ ETSAirspeed::collect()
 		return -1;
         }
 
-	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
+	if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
 	} else {
-		diff_pres_pa -= _diff_pres_offset;
+		diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
 	}
 
+	// The raw value still should be compensated for the known offset
+	diff_pres_pa_raw -= _diff_pres_offset;
+
 	// Track maximum differential pressure measured (so we can work out top speed).
 	if (diff_pres_pa > _max_differential_pressure_pa) {
 		_max_differential_pressure_pa = diff_pres_pa;
 	}
 
-	// XXX we may want to smooth out the readings to remove noise.
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_comms_errors);
 	report.differential_pressure_pa = (float)diff_pres_pa;
-	report.voltage = 0;
+
+	// XXX we may want to smooth out the readings to remove noise.
+	report.differential_pressure_filtered_pa = (float)diff_pres_pa;
+	report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
+	report.temperature = -1000.0f;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
 	if (_airspeed_pub > 0 && !(_pub_blocked)) {
@@ -204,14 +211,18 @@ ETSAirspeed::collect()
 void
 ETSAirspeed::cycle()
 {
+	int ret;
+
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
-		if (OK != collect()) {
+		ret = collect();
+		if (OK != ret) {
 			perf_count(_comms_errors);
 			/* restart the measurement state machine */
 			start();
+			_sensor_ok = false;
 			return;
 		}
 
@@ -235,8 +246,12 @@ ETSAirspeed::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
-		log("measure error");
+	ret = measure();
+	if (OK != ret) {
+		debug("measure error");
+	}
+
+	_sensor_ok = (ret == OK);
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -307,7 +322,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "no ETS airspeed sensor connected");
 }
 
 /**
@@ -351,7 +366,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %d pa", report.differential_pressure_pa);
+	warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -376,7 +391,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %d pa", report.differential_pressure_pa);
+		warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
 	}
 
 	/* reset the sensor polling to its default rate */
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 63b2d2d29..dd9b90fb3 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -53,6 +53,8 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
 
+#include <drivers/drv_hrt.h>
+
 /* FrSky sensor hub data IDs */
 #define FRSKY_ID_GPS_ALT_BP     0x01
 #define FRSKY_ID_TEMP1          0x02
@@ -192,17 +194,12 @@ void frsky_send_frame1(int uart)
 }
 
 /**
- * Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
+ * Formats the decimal latitude/longitude to the required degrees/minutes.
  */
 static float frsky_format_gps(float dec)
 {
-	float dms_deg = (int) dec;
-	float dec_deg = dec - dms_deg;
-	float dms_min = (int) (dec_deg * 60);
-	float dec_min = (dec_deg * 60) - dms_min;
-	float dms_sec = dec_min * 60;
-
-	return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
+	float dm_deg = (int) dec;
+	return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
 }
 
 /**
@@ -225,16 +222,16 @@ void frsky_send_frame2(int uart)
 	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
 	char lat_ns = 0, lon_ew = 0;
 	int sec = 0;
-	if (global_pos.valid) {
+	if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
 		time_t time_gps = global_pos.time_gps_usec / 1000000;
 		struct tm *tm_gps = gmtime(&time_gps);
 
 		course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
-		lat    = frsky_format_gps(abs(global_pos.lat) / 10000000.0f);
+		lat    = frsky_format_gps(fabsf(global_pos.lat));
 		lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
-		lon    = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
+		lon    = frsky_format_gps(fabsf(global_pos.lon));
 		lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
-		speed  = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy)
+		speed  = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e)
 				* 25.0f / 46.0f;
 		alt    = global_pos.alt;
 		sec    = tm_gps->tm_sec;
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 7b08ca69e..6e0839043 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -222,7 +222,7 @@ int frsky_telemetry_main(int argc, char *argv[])
 		frsky_task = task_spawn_cmd("frsky_telemetry",
 					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_DEFAULT,
-					  2048,
+					  2000,
 					  frsky_telemetry_thread_main,
 					  (const char **)argv);
 
diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk
index 1632c74f7..78ad6f67e 100644
--- a/src/drivers/frsky_telemetry/module.mk
+++ b/src/drivers/frsky_telemetry/module.mk
@@ -39,3 +39,7 @@ MODULE_COMMAND	= frsky_telemetry
 
 SRCS		= frsky_data.c \
               frsky_telemetry.c
+
+MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a736cbdf6..34dd63086 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -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
@@ -56,12 +56,16 @@
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/i2c.h>
+#include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/err.h>
 #include <drivers/drv_gps.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
+
+#include <board_config.h>
 
 #include "ubx.h"
 #include "mtk.h"
@@ -76,16 +80,18 @@
 #endif
 static const int ERROR = -1;
 
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
+/* class for dynamic allocation of satellite info data */
+class GPS_Sat_Info
+{
+public:
+	struct satellite_info_s 	_data;
+};
 
 
 class GPS : public device::CDev
 {
 public:
-	GPS(const char *uart_path, bool fake_gps);
+	GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -103,14 +109,17 @@ private:
 	int				_serial_fd;					///< serial interface to GPS
 	unsigned			_baudrate;					///< current baudrate
 	char				_port[20];					///< device / serial port path
-	volatile int			_task;						//< worker task
+	volatile int			_task;						///< worker task
 	bool				_healthy;					///< flag to signal if the GPS is ok
-	bool 				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
+	bool				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
 	bool				_mode_changed;					///< flag that the GPS mode has changed
 	gps_driver_mode_t		_mode;						///< current mode
 	GPS_Helper			*_Helper;					///< instance of GPS parser
-	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
-	orb_advert_t			_report_pub;					///< uORB pub for gps position
+	GPS_Sat_Info			*_Sat_Info;					///< instance of GPS sat info data object
+	struct vehicle_gps_position_s	_report_gps_pos;				///< uORB topic for gps position
+	orb_advert_t			_report_gps_pos_pub;				///< uORB pub for gps position
+	struct satellite_info_s		*_p_report_sat_info;				///< pointer to uORB topic for satellite info
+	orb_advert_t			_report_sat_info_pub;				///< uORB pub for satellite info
 	float				_rate;						///< position update rate
 	bool				_fake_gps;					///< fake gps output
 
@@ -118,7 +127,7 @@ private:
 	/**
 	 * Try to configure the GPS, handle outgoing communication to the GPS
 	 */
-	void				config();
+	void			 	config();
 
 	/**
 	 * Trampoline to the worker task
@@ -157,14 +166,17 @@ GPS	*g_dev;
 }
 
 
-GPS::GPS(const char *uart_path, bool fake_gps) :
+GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
 	_healthy(false),
 	_mode_changed(false),
 	_mode(GPS_DRIVER_MODE_UBX),
 	_Helper(nullptr),
-	_report_pub(-1),
+	_Sat_Info(nullptr),
+	_report_gps_pos_pub(-1),
+	_p_report_sat_info(nullptr),
+	_report_sat_info_pub(-1),
 	_rate(0.0f),
 	_fake_gps(fake_gps)
 {
@@ -175,7 +187,14 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
 
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
-	memset(&_report, 0, sizeof(_report));
+	memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
+
+	/* create satellite info data object if requested */
+	if (enable_sat_info) {
+		_Sat_Info = new(GPS_Sat_Info);
+		_p_report_sat_info = &_Sat_Info->_data;
+		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
+	}
 
 	_debug_enabled = true;
 }
@@ -209,7 +228,8 @@ GPS::init()
 		goto out;
 
 	/* start the GPS driver worker task */
-	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+	_task = task_spawn_cmd("gps", SCHED_DEFAULT,
+				SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		warnx("task start failed: %d", errno);
@@ -232,6 +252,11 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCRESET:
 		cmd_reset();
 		break;
+
+	default:
+		/* give it to parent if no one wants it */
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
 	}
 
 	unlock();
@@ -268,33 +293,32 @@ GPS::task_main()
 
 		if (_fake_gps) {
 
-			_report.timestamp_position = hrt_absolute_time();
-			_report.lat = (int32_t)47.378301e7f;
-			_report.lon = (int32_t)8.538777e7f;
-			_report.alt = (int32_t)400e3f;
-			_report.timestamp_variance = hrt_absolute_time();
-			_report.s_variance_m_s = 10.0f;
-			_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.timestamp_velocity = hrt_absolute_time();
-			_report.vel_n_m_s = 0.0f;
-			_report.vel_e_m_s = 0.0f;
-			_report.vel_d_m_s = 0.0f;
-			_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
-			_report.cog_rad = 0.0f;
-			_report.vel_ned_valid = true;
+			_report_gps_pos.timestamp_position = hrt_absolute_time();
+			_report_gps_pos.lat = (int32_t)47.378301e7f;
+			_report_gps_pos.lon = (int32_t)8.538777e7f;
+			_report_gps_pos.alt = (int32_t)1200e3f;
+			_report_gps_pos.timestamp_variance = hrt_absolute_time();
+			_report_gps_pos.s_variance_m_s = 10.0f;
+			_report_gps_pos.c_variance_rad = 0.1f;
+			_report_gps_pos.fix_type = 3;
+			_report_gps_pos.eph = 0.9f;
+			_report_gps_pos.epv = 1.8f;
+			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
+			_report_gps_pos.vel_n_m_s = 0.0f;
+			_report_gps_pos.vel_e_m_s = 0.0f;
+			_report_gps_pos.vel_d_m_s = 0.0f;
+			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
+			_report_gps_pos.cog_rad = 0.0f;
+			_report_gps_pos.vel_ned_valid = true;
 
 			//no time and satellite information simulated
 
 			if (!(_pub_blocked)) {
-				if (_report_pub > 0) {
-					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+				if (_report_gps_pos_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
 				} else {
-					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+					_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
 				}
 			}
 
@@ -310,11 +334,11 @@ GPS::task_main()
 
 			switch (_mode) {
 			case GPS_DRIVER_MODE_UBX:
-				_Helper = new UBX(_serial_fd, &_report);
+				_Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
 				break;
 
 			case GPS_DRIVER_MODE_MTK:
-				_Helper = new MTK(_serial_fd, &_report);
+				_Helper = new MTK(_serial_fd, &_report_gps_pos);
 				break;
 
 			default:
@@ -329,20 +353,33 @@ GPS::task_main()
 				// GPS is obviously detected successfully, reset statistics
 				_Helper->reset_update_rates();
 
-				while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+				int helper_ret;
+				while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
 	//				lock();
 					/* opportunistic publishing - else invalid data would end up on the bus */
 
 					if (!(_pub_blocked)) {
-						if (_report_pub > 0) {
-							orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+						if (helper_ret & 1) {
+							if (_report_gps_pos_pub > 0) {
+								orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
+
+							} else {
+								_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+							}
+						}
+						if (_p_report_sat_info && (helper_ret & 2)) {
+							if (_report_sat_info_pub > 0) {
+								orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
 
-						} else {
-							_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+							} else {
+								_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
+							}
 						}
 					}
 
-					last_rate_count++;
+					if (helper_ret & 1) {	// consider only pos info updates for rate calculation */
+						last_rate_count++;
+					}
 
 					/* measure update rate every 5 seconds */
 					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
@@ -354,7 +391,7 @@ GPS::task_main()
 					}
 
 					if (!_healthy) {
-						char *mode_str = "unknown";
+						const char *mode_str = "unknown";
 
 						switch (_mode) {
 						case GPS_DRIVER_MODE_UBX:
@@ -416,7 +453,14 @@ GPS::task_main()
 void
 GPS::cmd_reset()
 {
-	//XXX add reset?
+#ifdef GPIO_GPS_NRESET
+	warnx("Toggling GPS reset pin");
+	stm32_configgpio(GPIO_GPS_NRESET);
+	stm32_gpiowrite(GPIO_GPS_NRESET, 0);
+	usleep(100);
+	stm32_gpiowrite(GPIO_GPS_NRESET, 1);
+	warnx("Toggled GPS reset pin");
+#endif
 }
 
 void
@@ -436,12 +480,15 @@ GPS::print_info()
 	}
 
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
-
-	if (_report.timestamp_position != 0) {
-		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("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
+
+	if (_report_gps_pos.timestamp_position != 0) {
+		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
+				_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
+		warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
+		warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
+			(double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
+		warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.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);
@@ -459,7 +506,7 @@ namespace gps
 
 GPS	*g_dev;
 
-void	start(const char *uart_path, bool fake_gps);
+void	start(const char *uart_path, bool fake_gps, bool enable_sat_info);
 void	stop();
 void	test();
 void	reset();
@@ -469,7 +516,7 @@ void	info();
  * Start the driver.
  */
 void
-start(const char *uart_path, bool fake_gps)
+start(const char *uart_path, bool fake_gps, bool enable_sat_info)
 {
 	int fd;
 
@@ -477,7 +524,7 @@ start(const char *uart_path, bool fake_gps)
 		errx(1, "already started");
 
 	/* create the driver */
-	g_dev = new GPS(uart_path, fake_gps);
+	g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -568,8 +615,9 @@ gps_main(int argc, char *argv[])
 {
 
 	/* set to default */
-	char *device_name = GPS_DEFAULT_UART_PORT;
+	const char *device_name = GPS_DEFAULT_UART_PORT;
 	bool fake_gps = false;
+	bool enable_sat_info = false;
 
 	/*
 	 * Start/load the driver.
@@ -591,7 +639,13 @@ gps_main(int argc, char *argv[])
 				fake_gps = true;
 		}
 
-		gps::start(device_name, fake_gps);
+		/* Detect sat info option */
+		for (int i = 2; i < argc; i++) {
+			if (!strcmp(argv[i], "-s"))
+				enable_sat_info = true;
+		}
+
+		gps::start(device_name, fake_gps, enable_sat_info);
 	}
 
 	if (!strcmp(argv[1], "stop"))
@@ -616,5 +670,5 @@ gps_main(int argc, char *argv[])
 		gps::info();
 
 out:
-	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
 }
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 2360ff39b..3b92f1bf4 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate()
 	return _rate_vel;
 }
 
-float
+void
 GPS_Helper::reset_update_rates()
 {
 	_rate_count_vel = 0;
@@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates()
 	_interval_rate_start = hrt_absolute_time();
 }
 
-float
+void
 GPS_Helper::store_update_rates()
 {
 	_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index cfb9e0d43..3623397b2 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -46,20 +46,24 @@
 class GPS_Helper
 {
 public:
+
+	GPS_Helper() {};
+	virtual ~GPS_Helper() {};
+
 	virtual int			configure(unsigned &baud) = 0;
 	virtual int 			receive(unsigned timeout) = 0;
 	int 				set_baudrate(const int &fd, unsigned baud);
 	float				get_position_update_rate();
 	float				get_velocity_update_rate();
-	float				reset_update_rates();
-	float				store_update_rates();
+	void				reset_update_rates();
+	void				store_update_rates();
 
 protected:
 	uint8_t _rate_count_lat_lon;
 	uint8_t _rate_count_vel;
 
-	float _rate_lat_lon;
-	float _rate_vel;
+	float _rate_lat_lon = 0.0f;
+	float _rate_vel = 0.0f;
 
 	uint64_t _interval_rate_start;
 };
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
index 82c67d40a..b00818424 100644
--- a/src/drivers/gps/module.mk
+++ b/src/drivers/gps/module.mk
@@ -41,3 +41,7 @@ SRCS		= gps.cpp \
 		  gps_helper.cpp \
 		  mtk.cpp \
 		  ubx.cpp
+
+MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index c90ecbe28..c4f4f7bec 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -249,15 +249,21 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 		warnx("mtk: unknown revision");
 		_gps_position->lat = 0;
 		_gps_position->lon = 0;
+
+		// Indicate this data is not usable and bail out
+		_gps_position->eph = 1000.0f;
+		_gps_position->epv = 1000.0f;
+		_gps_position->fix_type = 0;
+		return;
 	}
 
 	_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 / 100.0f; // from cm to m
+	_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
 	_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;
+	_gps_position->satellites_used = packet.satellites;
 
 	/* convert time and date information to unix timestamp */
 	struct tm timeinfo; //TODO: test this conversion
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 8a2afecb7..d0854f5e9 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 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,14 +34,18 @@
 /**
  * @file ubx.cpp
  *
- * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description
  * including Prototol Specification.
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  *
+ * @author Hannes Delago
+ *   (rework, add ubx7+ compatibility)
+ *
  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf
  */
 
 #include <assert.h>
@@ -55,21 +59,44 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
 #include <drivers/drv_hrt.h>
 
 #include "ubx.h"
 
-#define UBX_CONFIG_TIMEOUT		200		// ms, timeout for waiting ACK
-#define UBX_PACKET_TIMEOUT		2		// ms, if now data during this delay assume that full update received
+#define UBX_CONFIG_TIMEOUT	200		// ms, timeout for waiting ACK
+#define UBX_PACKET_TIMEOUT	2		// ms, if now data during this delay assume that full update received
 #define UBX_WAIT_BEFORE_READ	20		// ms, wait before reading to save read() calls
-#define DISABLE_MSG_INTERVAL	1000000	// us, try to disable message with this interval
+#define DISABLE_MSG_INTERVAL	1000000		// us, try to disable message with this interval
+
+#define MIN(X,Y)	((X) < (Y) ? (X) : (Y))
+#define SWAP16(X)	((((X) >>  8) & 0x00ff) | (((X) << 8) & 0xff00))
+
+#define FNV1_32_INIT	((uint32_t)0x811c9dc5)	// init value for FNV1 hash algorithm
+#define FNV1_32_PRIME	((uint32_t)0x01000193)	// magic prime for FNV1 hash algorithm
+
 
-UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+/**** Trace macros, disable for production builds */
+#define UBX_TRACE_PARSER(s, ...)	{/*printf(s, ## __VA_ARGS__);*/}	/* decoding progress in parse_char() */
+#define UBX_TRACE_RXMSG(s, ...)		{/*printf(s, ## __VA_ARGS__);*/}	/* Rx msgs in payload_rx_done() */
+#define UBX_TRACE_SVINFO(s, ...)	{/*printf(s, ## __VA_ARGS__);*/}	/* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */
+
+/**** Warning macros, disable to save memory */
+#define UBX_WARN(s, ...)		{warnx(s, ## __VA_ARGS__);}
+
+
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
 	_fd(fd),
 	_gps_position(gps_position),
+	_satellite_info(satellite_info),
 	_configured(false),
-	_waiting_for_ack(false),
-	_disable_cmd_last(0)
+	_ack_state(UBX_ACK_IDLE),
+	_got_posllh(false),
+	_got_velned(false),
+	_disable_cmd_last(0),
+	_ack_waiting_msg(0),
+	_ubx_version(0),
+	_use_nav_pvt(false)
 {
 	decode_init();
 }
@@ -83,175 +110,167 @@ UBX::configure(unsigned &baudrate)
 {
 	_configured = false;
 	/* try different baudrates */
-	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+	const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200};
 
-	int baud_i;
+	unsigned baud_i;
 
-	for (baud_i = 0; baud_i < 5; baud_i++) {
-		baudrate = baudrates_to_try[baud_i];
+	for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
+		baudrate = baudrates[baud_i];
 		set_baudrate(_fd, baudrate);
 
+		/* flush input and wait for at least 20 ms silence */
+		decode_init();
+		receive(20);
+		decode_init();
+
 		/* Send a CFG-PRT message to set the UBX protocol for in and out
-		 * and leave the baudrate as it is, we just want an ACK-ACK from this
-		 */
-		type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
-		/* Set everything else of the packet to 0, otherwise the module wont accept it */
-		memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
-
-		_message_class_needed = UBX_CLASS_CFG;
-		_message_id_needed = UBX_MESSAGE_CFG_PRT;
-
-		/* Define the package contents, don't change the baudrate */
-		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
-		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
-		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
-		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
-		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
-		cfg_prt_packet.baudRate		= baudrate;
-		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
-		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
-
-		send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
-
-		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		 * and leave the baudrate as it is, we just want an ACK-ACK for this */
+		memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
+		_buf.payload_tx_cfg_prt.portID		= UBX_TX_CFG_PRT_PORTID;
+		_buf.payload_tx_cfg_prt.mode		= UBX_TX_CFG_PRT_MODE;
+		_buf.payload_tx_cfg_prt.baudRate	= baudrate;
+		_buf.payload_tx_cfg_prt.inProtoMask	= UBX_TX_CFG_PRT_INPROTOMASK;
+		_buf.payload_tx_cfg_prt.outProtoMask	= UBX_TX_CFG_PRT_OUTPROTOMASK;
+
+		send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
+
+		if (wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) {
 			/* try next baudrate */
 			continue;
 		}
 
 		/* Send a CFG-PRT message again, this time change the baudrate */
+		memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
+		_buf.payload_tx_cfg_prt.portID		= UBX_TX_CFG_PRT_PORTID;
+		_buf.payload_tx_cfg_prt.mode		= UBX_TX_CFG_PRT_MODE;
+		_buf.payload_tx_cfg_prt.baudRate	= UBX_TX_CFG_PRT_BAUDRATE;
+		_buf.payload_tx_cfg_prt.inProtoMask	= UBX_TX_CFG_PRT_INPROTOMASK;
+		_buf.payload_tx_cfg_prt.outProtoMask	= UBX_TX_CFG_PRT_OUTPROTOMASK;
 
-		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
-		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
-		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
-		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
-		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
-		cfg_prt_packet.baudRate		= UBX_CFG_PRT_PAYLOAD_BAUDRATE;
-		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
-		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
-
-		send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
+		send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
 
 		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
-		wait_for_ack(UBX_CONFIG_TIMEOUT);
+		wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
 
-		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
-			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
-			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) {
+			set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE);
+			baudrate = UBX_TX_CFG_PRT_BAUDRATE;
 		}
 
 		/* at this point we have correct baudrate on both ends */
 		break;
 	}
 
-	if (baud_i >= 5) {
-		return 1;
+	if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
+		return 1;	// connection and/or baudrate detection failed
 	}
 
-	/* send a CFG-RATE message to define update rate */
-	type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
-	memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
+	/* Send a CFG-RATE message to define update rate */
+	memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
+	_buf.payload_tx_cfg_rate.measRate	= UBX_TX_CFG_RATE_MEASINTERVAL;
+	_buf.payload_tx_cfg_rate.navRate	= UBX_TX_CFG_RATE_NAVRATE;
+	_buf.payload_tx_cfg_rate.timeRef	= UBX_TX_CFG_RATE_TIMEREF;
 
-	_message_class_needed = UBX_CLASS_CFG;
-	_message_id_needed = UBX_MESSAGE_CFG_RATE;
+	send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_rate));
 
-	cfg_rate_packet.clsID		= UBX_CLASS_CFG;
-	cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
-	cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
-	cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
-	cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
-	cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
-
-	send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: configuration failed: RATE");
+	if (wait_for_ack(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
 	/* send a NAV5 message to set the options for the internal filter */
-	type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
-	memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
-
-	_message_class_needed = UBX_CLASS_CFG;
-	_message_id_needed = UBX_MESSAGE_CFG_NAV5;
+	memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5));
+	_buf.payload_tx_cfg_nav5.mask		= UBX_TX_CFG_NAV5_MASK;
+	_buf.payload_tx_cfg_nav5.dynModel	= UBX_TX_CFG_NAV5_DYNMODEL;
+	_buf.payload_tx_cfg_nav5.fixMode	= UBX_TX_CFG_NAV5_FIXMODE;
 
-	cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
-	cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
-	cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
-	cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
-	cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
-	cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
+	send_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5));
 
-	send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: configuration failed: NAV5");
+	if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
 	/* configure message rates */
 	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
 
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV POSLLH");
-		return 1;
+	/* try to set rate for NAV-PVT */
+	/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
+	configure_message_rate(UBX_MSG_NAV_PVT, 1);
+	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+		_use_nav_pvt = false;
+	} else {
+		_use_nav_pvt = true;
 	}
+	UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
+	if (!_use_nav_pvt) {
+		configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
-		return 1;
-	}
+		configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
+		configure_message_rate(UBX_MSG_NAV_SOL, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SOL");
-		return 1;
+		configure_message_rate(UBX_MSG_NAV_VELNED, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 	}
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV VELNED");
+	configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
+	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SVINFO");
+	configure_message_rate(UBX_MSG_MON_HW, 1);
+	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
+	/* request module version information by sending an empty MON-VER message */
+	send_message(UBX_MSG_MON_VER, nullptr, 0);
+
 	_configured = true;
 	return 0;
 }
 
-int
-UBX::wait_for_ack(unsigned timeout)
+int	// -1 = NAK, error or timeout, 0 = ACK
+UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
 {
-	_waiting_for_ack = true;
-	uint64_t time_started = hrt_absolute_time();
+	int ret = -1;
 
-	while (hrt_absolute_time() < time_started + timeout * 1000) {
-		if (receive(timeout) > 0) {
-			if (!_waiting_for_ack) {
-				return 1;
-			}
+	_ack_state = UBX_ACK_WAITING;
+	_ack_waiting_msg = msg;	// memorize sent msg class&ID for ACK check
+
+	hrt_abstime time_started = hrt_absolute_time();
+
+	while ((_ack_state == UBX_ACK_WAITING) && (hrt_absolute_time() < time_started + timeout * 1000)) {
+		receive(timeout);
+	}
 
+	if (_ack_state == UBX_ACK_GOT_ACK) {
+		ret = 0;	// ACK received ok
+	} else if (report) {
+		if (_ack_state == UBX_ACK_GOT_NAK) {
+			UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
 		} else {
-			return -1;	// timeout or error receiving, or NAK
+			UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
 		}
 	}
 
-	return -1;	// timeout
+	_ack_state = UBX_ACK_IDLE;
+	return ret;
 }
 
-int
-UBX::receive(unsigned timeout)
+int	// -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
+UBX::receive(const unsigned timeout)
 {
 	/* poll descriptor */
 	pollfd fds[1];
@@ -265,22 +284,25 @@ UBX::receive(unsigned timeout)
 
 	ssize_t count = 0;
 
-	bool handled = false;
+	int handled = 0;
 
 	while (true) {
+		bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled;
 
 		/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
-		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout);
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
-			warnx("ubx: poll error");
+			UBX_WARN("ubx poll() err");
 			return -1;
 
 		} else if (ret == 0) {
 			/* return success after short delay after receiving a packet or timeout after long delay */
-			if (handled) {
-				return 1;
+			if (ready_to_return) {
+				_got_posllh = false;
+				_got_velned = false;
+				return handled;
 
 			} else {
 				return -1;
@@ -300,419 +322,675 @@ UBX::receive(unsigned timeout)
 
 				/* pass received bytes to the packet decoder */
 				for (int i = 0; i < count; i++) {
-					if (parse_char(buf[i]) > 0) {
-						if (handle_message() > 0)
-							handled = true;
-					}
+					handled |= parse_char(buf[i]);
 				}
 			}
 		}
 
 		/* abort after timeout if no useful packets received */
 		if (time_started + timeout * 1000 < hrt_absolute_time()) {
-			warnx("ubx: timeout - no useful messages");
 			return -1;
 		}
 	}
 }
 
-int
-UBX::parse_char(uint8_t b)
+int	// 0 = decoding, 1 = message handled, 2 = sat info message handled
+UBX::parse_char(const uint8_t b)
 {
+	int ret = 0;
+
 	switch (_decode_state) {
-		/* First, look for sync1 */
-	case UBX_DECODE_UNINIT:
-		if (b == UBX_SYNC1) {
-			_decode_state = UBX_DECODE_GOT_SYNC1;
-		}
 
+	/* Expecting Sync1 */
+	case UBX_DECODE_SYNC1:
+		if (b == UBX_SYNC1) {	// Sync1 found --> expecting Sync2
+			UBX_TRACE_PARSER("\nA");
+			_decode_state = UBX_DECODE_SYNC2;
+		}
 		break;
 
-		/* Second, look for sync2 */
-	case UBX_DECODE_GOT_SYNC1:
-		if (b == UBX_SYNC2) {
-			_decode_state = UBX_DECODE_GOT_SYNC2;
+	/* Expecting Sync2 */
+	case UBX_DECODE_SYNC2:
+		if (b == UBX_SYNC2) {	// Sync2 found --> expecting Class
+			UBX_TRACE_PARSER("B");
+			_decode_state = UBX_DECODE_CLASS;
 
-		} else {
-			/* Second start symbol was wrong, reset state machine */
+		} else {		// Sync1 not followed by Sync2: reset parser
 			decode_init();
-			/* don't return error, it can be just false sync1 */
 		}
+		break;
 
+	/* Expecting Class */
+	case UBX_DECODE_CLASS:
+		UBX_TRACE_PARSER("C");
+		add_byte_to_checksum(b);  // checksum is calculated for everything except Sync and Checksum bytes
+		_rx_msg = b;
+		_decode_state = UBX_DECODE_ID;
 		break;
 
-		/* Now look for class */
-	case UBX_DECODE_GOT_SYNC2:
-		/* everything except sync1 and sync2 needs to be added to the checksum */
+	/* Expecting ID */
+	case UBX_DECODE_ID:
+		UBX_TRACE_PARSER("D");
 		add_byte_to_checksum(b);
-		_message_class = b;
-		_decode_state = UBX_DECODE_GOT_CLASS;
+		_rx_msg |= b << 8;
+		_decode_state = UBX_DECODE_LENGTH1;
 		break;
 
-	case UBX_DECODE_GOT_CLASS:
+	/* Expecting first length byte */
+	case UBX_DECODE_LENGTH1:
+		UBX_TRACE_PARSER("E");
 		add_byte_to_checksum(b);
-		_message_id = b;
-		_decode_state = UBX_DECODE_GOT_MESSAGEID;
+		_rx_payload_length = b;
+		_decode_state = UBX_DECODE_LENGTH2;
 		break;
 
-	case UBX_DECODE_GOT_MESSAGEID:
+	/* Expecting second length byte */
+	case UBX_DECODE_LENGTH2:
+		UBX_TRACE_PARSER("F");
 		add_byte_to_checksum(b);
-		_payload_size = b; //this is the first length byte
-		_decode_state = UBX_DECODE_GOT_LENGTH1;
+		_rx_payload_length |= b << 8;	// calculate payload size
+		if (payload_rx_init() != 0) {	// start payload reception
+			// payload will not be handled, discard message
+			decode_init();
+		} else {
+			_decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
+		}
 		break;
 
-	case UBX_DECODE_GOT_LENGTH1:
+	/* Expecting payload */
+	case UBX_DECODE_PAYLOAD:
+		UBX_TRACE_PARSER(".");
 		add_byte_to_checksum(b);
-		_payload_size += b << 8; // here comes the second byte of length
-		_decode_state = UBX_DECODE_GOT_LENGTH2;
+		switch (_rx_msg) {
+		case UBX_MSG_NAV_SVINFO:
+			ret = payload_rx_add_nav_svinfo(b);	// add a NAV-SVINFO payload byte
+			break;
+		case UBX_MSG_MON_VER:
+			ret = payload_rx_add_mon_ver(b);	// add a MON-VER payload byte
+			break;
+		default:
+			ret = payload_rx_add(b);		// add a payload byte
+			break;
+		}
+		if (ret < 0) {
+			// payload not handled, discard message
+			decode_init();
+		} else if (ret > 0) {
+			// payload complete, expecting checksum
+			_decode_state = UBX_DECODE_CHKSUM1;
+		} else {
+			// expecting more payload, stay in state UBX_DECODE_PAYLOAD
+		}
+		ret = 0;
 		break;
 
-	case UBX_DECODE_GOT_LENGTH2:
+	/* Expecting first checksum byte */
+	case UBX_DECODE_CHKSUM1:
+		if (_rx_ck_a != b) {
+			UBX_WARN("ubx checksum err");
+			decode_init();
+		} else {
+			_decode_state = UBX_DECODE_CHKSUM2;
+		}
+		break;
 
-		/* Add to checksum if not yet at checksum byte */
-		if (_rx_count < _payload_size)
-			add_byte_to_checksum(b);
+	/* Expecting second checksum byte */
+	case UBX_DECODE_CHKSUM2:
+		if (_rx_ck_b != b) {
+			UBX_WARN("ubx checksum err");
+		} else {
+			ret = payload_rx_done();	// finish payload processing
+		}
+		decode_init();
+		break;
 
-		_rx_buffer[_rx_count] = b;
+	default:
+		break;
+	}
 
-		/* once the payload has arrived, we can process the information */
-		if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
-			/* compare checksum */
-			if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
-				decode_init();
-				return 1;	// message received successfully
+	return ret;
+}
 
-			} else {
-				warnx("ubx: checksum wrong");
-				decode_init();
-				return -1;
-			}
+/**
+ * Start payload rx
+ */
+int	// -1 = abort, 0 = continue
+UBX::payload_rx_init()
+{
+	int ret = 0;
 
-		} else if (_rx_count < RECV_BUFFER_SIZE) {
-			_rx_count++;
+	_rx_state = UBX_RXMSG_HANDLE;	// handle by default
+
+	switch (_rx_msg) {
+	case UBX_MSG_NAV_PVT:
+		if (   (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7)		/* u-blox 7 msg format */
+		    && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8))	/* u-blox 8+ msg format */
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (!_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if not using NAV-PVT
+		break;
 
-		} else {
-			warnx("ubx: buffer full");
-			decode_init();
-			return -1;
-		}
+	case UBX_MSG_NAV_POSLLH:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
+		break;
+
+	case UBX_MSG_NAV_SOL:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
+		break;
+
+	case UBX_MSG_NAV_TIMEUTC:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
+		break;
+
+	case UBX_MSG_NAV_SVINFO:
+		if (_satellite_info == nullptr)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if sat info not requested
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else
+			memset(_satellite_info, 0, sizeof(*_satellite_info));	// initialize sat info
+		break;
+
+	case UBX_MSG_NAV_VELNED:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
+		break;
+
+	case UBX_MSG_MON_VER:
+		break;		// unconditionally handle this message
 
+	case UBX_MSG_MON_HW:
+		if (   (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t))	/* u-blox 6 msg format */
+		    && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t)))	/* u-blox 7+ msg format */
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		break;
+
+	case UBX_MSG_ACK_ACK:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if _configured
+		break;
+
+	case UBX_MSG_ACK_NAK:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if _configured
 		break;
 
 	default:
+		_rx_state = UBX_RXMSG_DISABLE;	// disable all other messages
 		break;
 	}
 
-	return 0;	// message decoding in progress
+	switch (_rx_state) {
+	case UBX_RXMSG_HANDLE:	// handle message
+	case UBX_RXMSG_IGNORE:	// ignore message but don't report error
+		ret = 0;
+		break;
+
+	case UBX_RXMSG_DISABLE:	// disable unexpected messages
+		UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+
+		{
+			hrt_abstime t = hrt_absolute_time();
+
+			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
+				/* don't attempt for every message to disable, some might not be disabled */
+				_disable_cmd_last = t;
+				UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
+				configure_message_rate(_rx_msg, 0);
+			}
+		}
+
+		ret = -1;	// return error, abort handling this message
+		break;
+
+	case UBX_RXMSG_ERROR_LENGTH:	// error: invalid length
+		UBX_WARN("ubx msg 0x%04x invalid len %u", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+		ret = -1;	// return error, abort handling this message
+		break;
+
+	default:	// invalid message state
+		UBX_WARN("ubx internal err1");
+		ret = -1;	// return error, abort handling this message
+		break;
+	}
+
+	return ret;
 }
 
+/**
+ * Add payload rx byte
+ */
+int	// -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add(const uint8_t b)
+{
+	int ret = 0;
 
+	_buf.raw[_rx_payload_index] = b;
 
-int
-UBX::handle_message()
+	if (++_rx_payload_index >= _rx_payload_length) {
+		ret = 1;	// payload received completely
+	}
+
+	return ret;
+}
+
+/**
+ * Add NAV-SVINFO payload rx byte
+ */
+int	// -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add_nav_svinfo(const uint8_t b)
 {
 	int ret = 0;
 
-	if (_configured) {
-		/* handle only info messages when configured */
-		switch (_message_class) {
-		case UBX_CLASS_NAV:
-			switch (_message_id) {
-			case UBX_MESSAGE_NAV_POSLLH: {
-					// printf("GOT NAV_POSLLH\n");
-					gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
-
-					_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->timestamp_position = hrt_absolute_time();
-
-					_rate_count_lat_lon++;
-
-					ret = 1;
-					break;
-				}
+	if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
+		// Fill Part 1 buffer
+		_buf.raw[_rx_payload_index] = b;
+	} else {
+		if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
+			// Part 1 complete: decode Part 1 buffer
+			_satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, SAT_INFO_MAX_SATELLITES);
+			UBX_TRACE_SVINFO("SVINFO len %u  numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
+		}
+		if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) {
+			// Still room in _satellite_info: fill Part 2 buffer
+			unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t);
+			_buf.raw[buf_index] = b;
+			if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) {
+				// Part 2 complete: decode Part 2 buffer
+				unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t);
+				_satellite_info->used[sat_index]	= (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01);
+				_satellite_info->snr[sat_index]		= (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno);
+				_satellite_info->elevation[sat_index]	= (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev);
+				_satellite_info->azimuth[sat_index]	= (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f);
+				_satellite_info->svid[sat_index]	= (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid);
+				UBX_TRACE_SVINFO("SVINFO #%02u  used %u  snr %3u  elevation %3u  azimuth %3u  svid %3u\n",
+						(unsigned)sat_index + 1,
+						(unsigned)_satellite_info->used[sat_index],
+						(unsigned)_satellite_info->snr[sat_index],
+						(unsigned)_satellite_info->elevation[sat_index],
+						(unsigned)_satellite_info->azimuth[sat_index],
+						(unsigned)_satellite_info->svid[sat_index]
+				);
+			}
+		}
+	}
 
-			case UBX_MESSAGE_NAV_SOL: {
-					// printf("GOT NAV_SOL\n");
-					gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+	if (++_rx_payload_index >= _rx_payload_length) {
+		ret = 1;	// payload received completely
+	}
+
+	return ret;
+}
 
-					_gps_position->fix_type = packet->gpsFix;
-					_gps_position->s_variance_m_s = packet->sAcc;
-					_gps_position->p_variance_m = packet->pAcc;
-					_gps_position->timestamp_variance = hrt_absolute_time();
+/**
+ * Add MON-VER payload rx byte
+ */
+int	// -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add_mon_ver(const uint8_t b)
+{
+	int ret = 0;
 
-					ret = 1;
-					break;
-				}
+	if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+		// Fill Part 1 buffer
+		_buf.raw[_rx_payload_index] = b;
+	} else {
+		if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+			// Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
+			_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT);
+			_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version);
+			UBX_WARN("VER hash 0x%08x", _ubx_version);
+			UBX_WARN("VER hw  \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
+			UBX_WARN("VER sw  \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
+		}
+		// fill Part 2 buffer
+		unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
+		_buf.raw[buf_index] = b;
+		if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
+			// Part 2 complete: decode Part 2 buffer
+			UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
+		}
+	}
 
-			case UBX_MESSAGE_NAV_TIMEUTC: {
-					// printf("GOT NAV_TIMEUTC\n");
-					gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
+	if (++_rx_payload_index >= _rx_payload_length) {
+		ret = 1;	// payload received completely
+	}
 
-					/* convert to unix timestamp */
-					struct tm timeinfo;
-					timeinfo.tm_year = packet->year - 1900;
-					timeinfo.tm_mon = packet->month - 1;
-					timeinfo.tm_mday = packet->day;
-					timeinfo.tm_hour = packet->hour;
-					timeinfo.tm_min = packet->min;
-					timeinfo.tm_sec = packet->sec;
-					time_t epoch = mktime(&timeinfo);
+	return ret;
+}
+
+/**
+ * Finish payload rx
+ */
+int	// 0 = no message handled, 1 = message handled, 2 = sat info message handled
+UBX::payload_rx_done(void)
+{
+	int ret = 0;
+
+	// return if no message handled
+	if (_rx_state != UBX_RXMSG_HANDLE) {
+		return ret;
+	}
+
+	// handle message
+	switch (_rx_msg) {
+
+	case UBX_MSG_NAV_PVT:
+		UBX_TRACE_RXMSG("Rx NAV-PVT\n");
+
+		_gps_position->fix_type		= _buf.payload_rx_nav_pvt.fixType;
+		_gps_position->satellites_used	= _buf.payload_rx_nav_pvt.numSV;
+
+		_gps_position->lat		= _buf.payload_rx_nav_pvt.lat;
+		_gps_position->lon		= _buf.payload_rx_nav_pvt.lon;
+		_gps_position->alt		= _buf.payload_rx_nav_pvt.hMSL;
+
+		_gps_position->eph		= (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f;
+		_gps_position->epv		= (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f;
+		_gps_position->s_variance_m_s	= (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f;
+
+		_gps_position->vel_m_s		= (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f;
+
+		_gps_position->vel_n_m_s	= (float)_buf.payload_rx_nav_pvt.velN * 1e-3f;
+		_gps_position->vel_e_m_s	= (float)_buf.payload_rx_nav_pvt.velE * 1e-3f;
+		_gps_position->vel_d_m_s	= (float)_buf.payload_rx_nav_pvt.velD * 1e-3f;
+		_gps_position->vel_ned_valid	= true;
+
+		_gps_position->cog_rad		= (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f;
+		_gps_position->c_variance_rad	= (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
+
+		{
+			/* convert to unix timestamp */
+			struct tm timeinfo;
+			timeinfo.tm_year	= _buf.payload_rx_nav_pvt.year - 1900;
+			timeinfo.tm_mon		= _buf.payload_rx_nav_pvt.month - 1;
+			timeinfo.tm_mday	= _buf.payload_rx_nav_pvt.day;
+			timeinfo.tm_hour	= _buf.payload_rx_nav_pvt.hour;
+			timeinfo.tm_min		= _buf.payload_rx_nav_pvt.min;
+			timeinfo.tm_sec		= _buf.payload_rx_nav_pvt.sec;
+			time_t epoch = mktime(&timeinfo);
 
 #ifndef CONFIG_RTC
-					//Since we lack a hardware RTC, set the system time clock based on GPS UTC
-					//TODO generalize this by moving into gps.cpp?
-					timespec ts;
-					ts.tv_sec = epoch;
-					ts.tv_nsec = packet->time_nanoseconds;
-					clock_settime(CLOCK_REALTIME, &ts);
+			//Since we lack a hardware RTC, set the system time clock based on GPS UTC
+			//TODO generalize this by moving into gps.cpp?
+			timespec ts;
+			ts.tv_sec = epoch;
+			ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
+			clock_settime(CLOCK_REALTIME, &ts);
 #endif
 
-					_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-					_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
-					_gps_position->timestamp_time = hrt_absolute_time();
+			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f);
+		}
 
-					ret = 1;
-					break;
-				}
+		_gps_position->timestamp_time		= hrt_absolute_time();
+		_gps_position->timestamp_velocity 	= hrt_absolute_time();
+		_gps_position->timestamp_variance 	= hrt_absolute_time();
+		_gps_position->timestamp_position	= hrt_absolute_time();
 
-			case UBX_MESSAGE_NAV_SVINFO: {
-					//printf("GOT NAV_SVINFO\n");
-					const int length_part1 = 8;
-					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
-					const int length_part2 = 12;
-					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-
-					uint8_t satellites_used = 0;
-					int i;
-
-					//printf("Number of Channels: %d\n", packet_part1->numCh);
-					for (i = 0; i < packet_part1->numCh; i++) {
-						/* set pointer to sattelite_i information */
-						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
-
-						/* write satellite information to global storage */
-						uint8_t sv_used = packet_part2->flags & 0x01;
-
-						if (sv_used) {
-							/* count SVs used for NAV */
-							satellites_used++;
-						}
-
-						/* record info for all channels, whether or not the SV is used for NAV */
-						_gps_position->satellite_used[i] = sv_used;
-						_gps_position->satellite_snr[i] = packet_part2->cno;
-						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
-						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
-						_gps_position->satellite_prn[i] = packet_part2->svid;
-						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
-					}
-
-					for (i = packet_part1->numCh; i < 20; i++) {
-						/* unused channels have to be set to zero for e.g. MAVLink */
-						_gps_position->satellite_prn[i] = 0;
-						_gps_position->satellite_used[i] = 0;
-						_gps_position->satellite_snr[i] = 0;
-						_gps_position->satellite_elevation[i] = 0;
-						_gps_position->satellite_azimuth[i] = 0;
-					}
-
-					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
-
-					if (packet_part1->numCh > 0) {
-						_gps_position->satellite_info_available = true;
-
-					} else {
-						_gps_position->satellite_info_available = false;
-					}
-
-					_gps_position->timestamp_satellites = hrt_absolute_time();
-
-					ret = 1;
-					break;
-				}
+		_rate_count_vel++;
+		_rate_count_lat_lon++;
 
-			case UBX_MESSAGE_NAV_VELNED: {
-					// printf("GOT NAV_VELNED\n");
-					gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
+		_got_posllh = true;
+		_got_velned = true;
 
-					_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
-					_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
-					_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
-					_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
-					_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
-					_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
-					_gps_position->vel_ned_valid = true;
-					_gps_position->timestamp_velocity = hrt_absolute_time();
+		ret = 1;
+		break;
 
-					_rate_count_vel++;
+	case UBX_MSG_NAV_POSLLH:
+		UBX_TRACE_RXMSG("Rx NAV-POSLLH\n");
 
-					ret = 1;
-					break;
-				}
+		_gps_position->lat	= _buf.payload_rx_nav_posllh.lat;
+		_gps_position->lon	= _buf.payload_rx_nav_posllh.lon;
+		_gps_position->alt	= _buf.payload_rx_nav_posllh.hMSL;
+		_gps_position->eph	= (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m
+		_gps_position->epv	= (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m
 
-			default:
-				break;
-			}
+		_gps_position->timestamp_position = hrt_absolute_time();
+
+		_rate_count_lat_lon++;
+		_got_posllh = true;
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_NAV_SOL:
+		UBX_TRACE_RXMSG("Rx NAV-SOL\n");
+
+		_gps_position->fix_type		= _buf.payload_rx_nav_sol.gpsFix;
+		_gps_position->s_variance_m_s	= (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f;	// from cm to m
+		_gps_position->satellites_used	= _buf.payload_rx_nav_sol.numSV;
+
+		_gps_position->timestamp_variance = hrt_absolute_time();
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_NAV_TIMEUTC:
+		UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
 
+		{
+			/* convert to unix timestamp */
+			struct tm timeinfo;
+			timeinfo.tm_year	= _buf.payload_rx_nav_timeutc.year - 1900;
+			timeinfo.tm_mon		= _buf.payload_rx_nav_timeutc.month - 1;
+			timeinfo.tm_mday	= _buf.payload_rx_nav_timeutc.day;
+			timeinfo.tm_hour	= _buf.payload_rx_nav_timeutc.hour;
+			timeinfo.tm_min		= _buf.payload_rx_nav_timeutc.min;
+			timeinfo.tm_sec		= _buf.payload_rx_nav_timeutc.sec;
+			time_t epoch = mktime(&timeinfo);
+
+#ifndef CONFIG_RTC
+			//Since we lack a hardware RTC, set the system time clock based on GPS UTC
+			//TODO generalize this by moving into gps.cpp?
+			timespec ts;
+			ts.tv_sec = epoch;
+			ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
+			clock_settime(CLOCK_REALTIME, &ts);
+#endif
+
+			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f);
+		}
+
+		_gps_position->timestamp_time = hrt_absolute_time();
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_NAV_SVINFO:
+		UBX_TRACE_RXMSG("Rx NAV-SVINFO\n");
+
+		// _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp
+		_satellite_info->timestamp = hrt_absolute_time();
+
+		ret = 2;
+		break;
+
+	case UBX_MSG_NAV_VELNED:
+		UBX_TRACE_RXMSG("Rx NAV-VELNED\n");
+
+		_gps_position->vel_m_s		= (float)_buf.payload_rx_nav_velned.speed * 1e-2f;
+		_gps_position->vel_n_m_s	= (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */
+		_gps_position->vel_e_m_s	= (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */
+		_gps_position->vel_d_m_s	= (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */
+		_gps_position->cog_rad		= (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f;
+		_gps_position->c_variance_rad	= (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f;
+		_gps_position->vel_ned_valid	= true;
+
+		_gps_position->timestamp_velocity = hrt_absolute_time();
+
+		_rate_count_vel++;
+		_got_velned = true;
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_MON_VER:
+		UBX_TRACE_RXMSG("Rx MON-VER\n");
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_MON_HW:
+		UBX_TRACE_RXMSG("Rx MON-HW\n");
+
+		switch (_rx_payload_length) {
+
+		case sizeof(ubx_payload_rx_mon_hw_ubx6_t):	/* u-blox 6 msg format */
+			_gps_position->noise_per_ms		= _buf.payload_rx_mon_hw_ubx6.noisePerMS;
+			_gps_position->jamming_indicator	= _buf.payload_rx_mon_hw_ubx6.jamInd;
+
+			ret = 1;
 			break;
 
-		case UBX_CLASS_ACK: {
-				/* ignore ACK when already configured */
-				ret = 1;
-				break;
-			}
+		case sizeof(ubx_payload_rx_mon_hw_ubx7_t):	/* u-blox 7+ msg format */
+			_gps_position->noise_per_ms		= _buf.payload_rx_mon_hw_ubx7.noisePerMS;
+			_gps_position->jamming_indicator	= _buf.payload_rx_mon_hw_ubx7.jamInd;
 
-		default:
+			ret = 1;
 			break;
-		}
 
-		if (ret == 0) {
-			/* message not handled */
-			warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+		default:		// unexpected payload size:
+			ret = 0;	// don't handle message
+			break;
+		}
+		break;
 
-			hrt_abstime t = hrt_absolute_time();
+	case UBX_MSG_ACK_ACK:
+		UBX_TRACE_RXMSG("Rx ACK-ACK\n");
 
-			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
-				/* don't attempt for every message to disable, some might not be disabled */
-				_disable_cmd_last = t;
-				warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
-				configure_message_rate(_message_class, _message_id, 0);
-			}
+		if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
+			_ack_state = UBX_ACK_GOT_ACK;
 		}
 
-	} else {
-		/* handle only ACK while configuring */
-		if (_message_class == UBX_CLASS_ACK) {
-			switch (_message_id) {
-			case UBX_MESSAGE_ACK_ACK: {
-					// printf("GOT ACK_ACK\n");
-					gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
-
-					if (_waiting_for_ack) {
-						if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) {
-							_waiting_for_ack = false;
-							ret = 1;
-						}
-					}
-
-					break;
-				}
+		ret = 1;
+		break;
 
-			case UBX_MESSAGE_ACK_NAK: {
-					// printf("GOT ACK_NAK\n");
-					warnx("ubx: not acknowledged");
-					/* configuration obviously not successful */
-					_waiting_for_ack = false;
-					ret = -1;
-					break;
-				}
+	case UBX_MSG_ACK_NAK:
+		UBX_TRACE_RXMSG("Rx ACK-NAK\n");
 
-			default:
-				break;
-			}
+		if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
+			_ack_state = UBX_ACK_GOT_NAK;
 		}
+
+		ret = 1;
+		break;
+
+	default:
+		break;
 	}
 
-	decode_init();
 	return ret;
 }
 
 void
 UBX::decode_init(void)
 {
+	_decode_state = UBX_DECODE_SYNC1;
 	_rx_ck_a = 0;
 	_rx_ck_b = 0;
-	_rx_count = 0;
-	_decode_state = UBX_DECODE_UNINIT;
-	_payload_size = 0;
-	/* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */
+	_rx_payload_length = 0;
+	_rx_payload_index = 0;
 }
 
 void
-UBX::add_byte_to_checksum(uint8_t b)
+UBX::add_byte_to_checksum(const uint8_t b)
 {
 	_rx_ck_a = _rx_ck_a + b;
 	_rx_ck_b = _rx_ck_b + _rx_ck_a;
 }
 
 void
-UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
+UBX::calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum)
 {
-	uint8_t ck_a = 0;
-	uint8_t ck_b = 0;
-	unsigned i;
-
-	for (i = 0; i < length - 2; i++) {
-		ck_a = ck_a + message[i];
-		ck_b = ck_b + ck_a;
+	for (uint16_t i = 0; i < length; i++) {
+		checksum->ck_a = checksum->ck_a + buffer[i];
+		checksum->ck_b = checksum->ck_b + checksum->ck_a;
 	}
-
-	/* the checksum is written to the last to bytes of a message */
-	message[length - 2] = ck_a;
-	message[length - 1] = ck_b;
 }
 
 void
-UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
+UBX::configure_message_rate(const uint16_t msg, const uint8_t rate)
 {
-	for (unsigned i = 0; i < length; i++) {
-		ck_a = ck_a + message[i];
-		ck_b = ck_b + ck_a;
-	}
+	ubx_payload_tx_cfg_msg_t cfg_msg;	// don't use _buf (allow interleaved operation)
+
+	cfg_msg.msg	= msg;
+	cfg_msg.rate	= rate;
+
+	send_message(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg));
 }
 
 void
-UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
+UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length)
 {
-	struct ubx_cfg_msg_rate msg;
-	msg.msg_class	= msg_class;
-	msg.msg_id		= msg_id;
-	msg.rate		= rate;
-	send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+	ubx_header_t   header = {UBX_SYNC1, UBX_SYNC2};
+	ubx_checksum_t checksum = {0, 0};
+
+	// Populate header
+	header.msg	= msg;
+	header.length	= length;
+
+	// Calculate checksum
+	calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum);  // skip 2 sync bytes
+	if (payload != nullptr)
+		calc_checksum(payload, length, &checksum);
+
+	// Send message
+	write(_fd, (const void *)&header, sizeof(header));
+	if (payload != nullptr)
+		write(_fd, (const void *)payload, length);
+	write(_fd, (const void *)&checksum, sizeof(checksum));
 }
 
-void
-UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
+uint32_t
+UBX::fnv1_32_str(uint8_t *str, uint32_t hval)
 {
-	ssize_t ret = 0;
-
-	/* calculate the checksum now */
-	add_checksum_to_message(packet, length);
-
-	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
+    uint8_t *s = str;
+
+    /*
+     * FNV-1 hash each octet in the buffer
+     */
+    while (*s) {
+
+	/* multiply by the 32 bit FNV magic prime mod 2^32 */
+#if defined(NO_FNV_GCC_OPTIMIZATION)
+	hval *= FNV1_32_PRIME;
+#else
+	hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24);
+#endif
 
-	/* start with the two sync bytes */
-	ret += write(fd, sync_bytes, sizeof(sync_bytes));
-	ret += write(fd, packet, length);
+	/* xor the bottom with the current octet */
+	hval ^= (uint32_t)*s++;
+    }
 
-	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
-		warnx("ubx: configuration write fail");
+    /* return our new hash value */
+    return hval;
 }
 
-void
-UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
-{
-	struct ubx_header header;
-	uint8_t ck_a = 0, ck_b = 0;
-	header.sync1 = UBX_SYNC1;
-	header.sync2 = UBX_SYNC2;
-	header.msg_class = msg_class;
-	header.msg_id    = msg_id;
-	header.length    = size;
-
-	add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
-	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
-
-	/* configure ACK check */
-	_message_class_needed = msg_class;
-	_message_id_needed = msg_id;
-
-	write(_fd, (const char *)&header, sizeof(header));
-	write(_fd, (const char *)msg, size);
-	write(_fd, (const char *)&ck_a, 1);
-	write(_fd, (const char *)&ck_b, 1);
-}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 79a904f4a..219a5762a 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 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,13 +34,16 @@
 /**
  * @file ubx.h
  *
- * U-Blox protocol definition. Following u-blox 6/7 Receiver Description
+ * U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description
  * including Prototol Specification.
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  *
+ * @author Hannes Delago
+ *   (rework, add ubx7+ compatibility)
+ *
  */
 
 #ifndef UBX_H_
@@ -51,295 +54,433 @@
 #define UBX_SYNC1 0xB5
 #define UBX_SYNC2 0x62
 
-/* ClassIDs (the ones that are used) */
-#define UBX_CLASS_NAV 0x01
-//#define UBX_CLASS_RXM 0x02
-#define UBX_CLASS_ACK 0x05
-#define UBX_CLASS_CFG 0x06
-
-/* MessageIDs (the ones that are used) */
-#define UBX_MESSAGE_NAV_POSLLH 0x02
-//#define UBX_MESSAGE_NAV_DOP 0x04
-#define UBX_MESSAGE_NAV_SOL 0x06
-#define UBX_MESSAGE_NAV_VELNED 0x12
-//#define UBX_MESSAGE_RXM_SVSI 0x20
-#define UBX_MESSAGE_NAV_TIMEUTC 0x21
-#define UBX_MESSAGE_NAV_SVINFO 0x30
-#define UBX_MESSAGE_ACK_NAK 0x00
-#define UBX_MESSAGE_ACK_ACK 0x01
-#define UBX_MESSAGE_CFG_PRT 0x00
-#define UBX_MESSAGE_CFG_MSG 0x01
-#define UBX_MESSAGE_CFG_RATE 0x08
-#define UBX_MESSAGE_CFG_NAV5 0x24
-
-#define UBX_CFG_PRT_LENGTH 20
-#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01			/**< UART1 */
-#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0		/**< 0b0000100011010000: 8N1 */
-#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400		/**< choose 38400 as GPS baudrate */
-#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01		/**< UBX in */
-#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01		/**< UBX out */
-
-#define UBX_CFG_RATE_LENGTH 6
-#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200		/**< 200ms for 5Hz */
-#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
-#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
-
-
-#define UBX_CFG_NAV5_LENGTH 36
-#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005		/**< XXX only update dynamic model and fix mode */
-#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
-#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2			/**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
-
-#define UBX_CFG_MSG_LENGTH 8
-#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
-#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
-#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
-
-#define UBX_MAX_PAYLOAD_LENGTH 500
-
-// ************
-/** the structures of the binary packets */
+/* Message Classes */
+#define UBX_CLASS_NAV		0x01
+#define UBX_CLASS_ACK		0x05
+#define UBX_CLASS_CFG		0x06
+#define UBX_CLASS_MON		0x0A
+
+/* Message IDs */
+#define UBX_ID_NAV_POSLLH	0x02
+#define UBX_ID_NAV_SOL		0x06
+#define UBX_ID_NAV_PVT		0x07
+#define UBX_ID_NAV_VELNED	0x12
+#define UBX_ID_NAV_TIMEUTC	0x21
+#define UBX_ID_NAV_SVINFO	0x30
+#define UBX_ID_ACK_NAK		0x00
+#define UBX_ID_ACK_ACK		0x01
+#define UBX_ID_CFG_PRT		0x00
+#define UBX_ID_CFG_MSG		0x01
+#define UBX_ID_CFG_RATE		0x08
+#define UBX_ID_CFG_NAV5		0x24
+#define UBX_ID_MON_VER		0x04
+#define UBX_ID_MON_HW		0x09
+
+/* Message Classes & IDs */
+#define UBX_MSG_NAV_POSLLH	((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8)
+#define UBX_MSG_NAV_SOL		((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8)
+#define UBX_MSG_NAV_PVT		((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8)
+#define UBX_MSG_NAV_VELNED	((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8)
+#define UBX_MSG_NAV_TIMEUTC	((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8)
+#define UBX_MSG_NAV_SVINFO	((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8)
+#define UBX_MSG_ACK_NAK		((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8)
+#define UBX_MSG_ACK_ACK		((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8)
+#define UBX_MSG_CFG_PRT		((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8)
+#define UBX_MSG_CFG_MSG		((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
+#define UBX_MSG_CFG_RATE	((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
+#define UBX_MSG_CFG_NAV5	((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
+#define UBX_MSG_MON_HW		((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
+#define UBX_MSG_MON_VER		((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
+
+/* RX NAV-PVT message content details */
+/*   Bitfield "valid" masks */
+#define UBX_RX_NAV_PVT_VALID_VALIDDATE		0x01	/**< validDate (Valid UTC Date) */
+#define UBX_RX_NAV_PVT_VALID_VALIDTIME		0x02	/**< validTime (Valid UTC Time) */
+#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED	0x04	/**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */
+
+/*   Bitfield "flags" masks */
+#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK		0x01	/**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */
+#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN		0x02	/**< diffSoln (1 if differential corrections were applied) */
+#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE		0x1C	/**< psmState (Power Save Mode state (see Power Management)) */
+#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID	0x20	/**< headVehValid (Heading of vehicle is valid) */
+
+/* RX NAV-TIMEUTC message content details */
+/*   Bitfield "valid" masks */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW	0x01	/**< validTOW (1 = Valid Time of Week) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN	0x02	/**< validWKN (1 = Valid Week Number) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC	0x04	/**< validUTC (1 = Valid UTC Time) */
+#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD	0xF0	/**< utcStandard (0..15 = UTC standard identifier) */
+
+/* TX CFG-PRT message contents */
+#define UBX_TX_CFG_PRT_PORTID		0x01		/**< UART1 */
+#define UBX_TX_CFG_PRT_MODE		0x000008D0	/**< 0b0000100011010000: 8N1 */
+#define UBX_TX_CFG_PRT_BAUDRATE		38400		/**< choose 38400 as GPS baudrate */
+#define UBX_TX_CFG_PRT_INPROTOMASK	0x01		/**< UBX in */
+#define UBX_TX_CFG_PRT_OUTPROTOMASK	0x01		/**< UBX out */
+
+/* TX CFG-RATE message contents */
+#define UBX_TX_CFG_RATE_MEASINTERVAL	200		/**< 200ms for 5Hz */
+#define UBX_TX_CFG_RATE_NAVRATE		1		/**< cannot be changed */
+#define UBX_TX_CFG_RATE_TIMEREF		0		/**< 0: UTC, 1: GPS time */
+
+/* TX CFG-NAV5 message contents */
+#define UBX_TX_CFG_NAV5_MASK		0x0005		/**< Only update dynamic model and fix mode */
+#define UBX_TX_CFG_NAV5_DYNMODEL	7		/**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
+#define UBX_TX_CFG_NAV5_FIXMODE		2		/**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
+
+/* TX CFG-MSG message contents */
+#define UBX_TX_CFG_MSG_RATE1_5HZ	0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_TX_CFG_MSG_RATE1_1HZ	0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_TX_CFG_MSG_RATE1_05HZ	10
+
+
+/*** u-blox protocol binary message and payload definitions ***/
 #pragma pack(push, 1)
 
-struct ubx_header {
-	uint8_t sync1;
-	uint8_t sync2;
-	uint8_t msg_class;
-	uint8_t msg_id;
-	uint16_t length;
-};
-
+/* General: Header */
 typedef struct {
-	uint32_t time_milliseconds;		/**<  GPS Millisecond Time of Week */
-	int32_t lon;					/**<  Longitude * 1e-7, deg */
-	int32_t lat;					/**<  Latitude * 1e-7, deg */
-	int32_t height;					/**<  Height above Ellipsoid, mm */
-	int32_t height_msl;				/**<  Height above mean sea level, mm */
-	uint32_t hAcc;  				/**< Horizontal Accuracy Estimate, mm */
-	uint32_t vAcc;  				/**< Vertical Accuracy Estimate, mm */
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_posllh_packet_t;
+	uint8_t		sync1;
+	uint8_t		sync2;
+	uint16_t	msg;
+	uint16_t	length;
+} ubx_header_t;
 
+/* General: Checksum */
 typedef struct {
-	uint32_t time_milliseconds; 	/**< GPS Millisecond Time of Week */
-	int32_t time_nanoseconds;		/**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
-	int16_t week;					/**< GPS week (GPS time) */
-	uint8_t gpsFix;					/**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
-	uint8_t flags;
-	int32_t ecefX;
-	int32_t ecefY;
-	int32_t ecefZ;
-	uint32_t pAcc;
-	int32_t ecefVX;
-	int32_t ecefVY;
-	int32_t ecefVZ;
-	uint32_t sAcc;
-	uint16_t pDOP;
-	uint8_t reserved1;
-	uint8_t numSV;
-	uint32_t reserved2;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_sol_packet_t;
+	uint8_t		ck_a;
+	uint8_t		ck_b;
+} ubx_checksum_t ;
 
+/* Rx NAV-POSLLH */
 typedef struct {
-	uint32_t time_milliseconds;		/**< GPS Millisecond Time of Week */
-	uint32_t time_accuracy; 		/**< Time Accuracy Estimate, ns */
-	int32_t time_nanoseconds; 		/**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
-	uint16_t year; 					/**< Year, range 1999..2099 (UTC) */
-	uint8_t month; 					/**< Month, range 1..12 (UTC) */
-	uint8_t day; 					/**< Day of Month, range 1..31 (UTC) */
-	uint8_t hour; 					/**< Hour of Day, range 0..23 (UTC) */
-	uint8_t min; 					/**< Minute of Hour, range 0..59 (UTC) */
-	uint8_t sec; 					/**< Seconds of Minute, range 0..59 (UTC) */
-	uint8_t valid_flag; 			/**< Validity Flags (see ubx documentation) */
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_timeutc_packet_t;
-
-//typedef struct {
-//	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
-//	uint16_t gDOP; 					/**< Geometric DOP (scaling 0.01) */
-//	uint16_t pDOP; 					/**< Position DOP (scaling 0.01) */
-//	uint16_t tDOP; 					/**< Time DOP (scaling 0.01) */
-//	uint16_t vDOP; 					/**< Vertical DOP (scaling 0.01) */
-//	uint16_t hDOP; 					/**< Horizontal DOP (scaling 0.01) */
-//	uint16_t nDOP; 					/**< Northing DOP (scaling 0.01) */
-//	uint16_t eDOP; 					/**< Easting DOP (scaling 0.01) */
-//	uint8_t ck_a;
-//	uint8_t ck_b;
-//} gps_bin_nav_dop_packet_t;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	int32_t		lon;		/**< Longitude [1e-7 deg] */
+	int32_t		lat;		/**< Latitude [1e-7 deg] */
+	int32_t		height;		/**< Height above ellipsoid [mm] */
+	int32_t		hMSL;		/**< Height above mean sea level [mm] */
+	uint32_t	hAcc;  		/**< Horizontal accuracy estimate [mm] */
+	uint32_t	vAcc;  		/**< Vertical accuracy estimate [mm] */
+} ubx_payload_rx_nav_posllh_t;
+
+/* Rx NAV-SOL */
 typedef struct {
-	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
-	uint8_t numCh; 					/**< Number of channels */
-	uint8_t globalFlags;
-	uint16_t reserved2;
-
-} gps_bin_nav_svinfo_part1_packet_t;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	int32_t		fTOW;		/**< Fractional part of iTOW (range: +/-500000) [ns] */
+	int16_t		week;		/**< GPS week */
+	uint8_t		gpsFix;		/**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
+	uint8_t		flags;
+	int32_t		ecefX;
+	int32_t		ecefY;
+	int32_t		ecefZ;
+	uint32_t	pAcc;
+	int32_t		ecefVX;
+	int32_t		ecefVY;
+	int32_t		ecefVZ;
+	uint32_t	sAcc;
+	uint16_t	pDOP;
+	uint8_t		reserved1;
+	uint8_t		numSV;		/**< Number of SVs used in Nav Solution */
+	uint32_t	reserved2;
+} ubx_payload_rx_nav_sol_t;
+
+/* Rx NAV-PVT (ubx8) */
 typedef struct {
-	uint8_t chn; 					/**< Channel number, 255 for SVs not assigned to a channel */
-	uint8_t svid; 					/**< Satellite ID */
-	uint8_t flags;
-	uint8_t quality;
-	uint8_t cno; 					/**< Carrier to Noise Ratio (Signal Strength), dbHz */
-	int8_t elev; 					/**< Elevation in integer degrees */
-	int16_t azim; 					/**< Azimuth in integer degrees */
-	int32_t prRes; 					/**< Pseudo range residual in centimetres */
-
-} gps_bin_nav_svinfo_part2_packet_t;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	uint16_t	year; 		/**< Year (UTC)*/
+	uint8_t		month; 		/**< Month, range 1..12 (UTC) */
+	uint8_t		day; 		/**< Day of month, range 1..31 (UTC) */
+	uint8_t		hour; 		/**< Hour of day, range 0..23 (UTC) */
+	uint8_t		min; 		/**< Minute of hour, range 0..59 (UTC) */
+	uint8_t		sec;		/**< Seconds of minute, range 0..60 (UTC) */
+	uint8_t		valid; 		/**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
+	uint32_t	tAcc; 		/**< Time accuracy estimate (UTC) [ns] */
+	int32_t		nano;		/**< Fraction of second (UTC) [-1e9...1e9 ns] */
+	uint8_t		fixType;	/**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
+	uint8_t		flags;		/**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */
+	uint8_t		reserved1;
+	uint8_t		numSV;		/**< Number of SVs used in Nav Solution */
+	int32_t		lon;		/**< Longitude [1e-7 deg] */
+	int32_t		lat;		/**< Latitude [1e-7 deg] */
+	int32_t		height;		/**< Height above ellipsoid [mm] */
+	int32_t		hMSL;		/**< Height above mean sea level [mm] */
+	uint32_t	hAcc;  		/**< Horizontal accuracy estimate [mm] */
+	uint32_t	vAcc;  		/**< Vertical accuracy estimate [mm] */
+	int32_t		velN;		/**< NED north velocity [mm/s]*/
+	int32_t		velE;		/**< NED east velocity [mm/s]*/
+	int32_t		velD;		/**< NED down velocity [mm/s]*/
+	int32_t		gSpeed;		/**< Ground Speed (2-D) [mm/s] */
+	int32_t		headMot;	/**< Heading of motion (2-D) [1e-5 deg] */
+	uint32_t	sAcc;		/**< Speed accuracy estimate [mm/s] */
+	uint32_t	headAcc;	/**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */
+	uint16_t	pDOP;		/**< Position DOP [0.01] */
+	uint16_t	reserved2;
+	uint32_t	reserved3;
+	int32_t		headVeh;	/**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */
+	uint32_t	reserved4;	/**< (ubx8+ only) */
+} ubx_payload_rx_nav_pvt_t;
+#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7	(sizeof(ubx_payload_rx_nav_pvt_t) - 8)
+#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8	(sizeof(ubx_payload_rx_nav_pvt_t))
+
+/* Rx NAV-TIMEUTC */
 typedef struct {
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_svinfo_part3_packet_t;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	uint32_t	tAcc; 		/**< Time accuracy estimate (UTC) [ns] */
+	int32_t		nano;		/**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */
+	uint16_t	year; 		/**< Year, range 1999..2099 (UTC) */
+	uint8_t		month; 		/**< Month, range 1..12 (UTC) */
+	uint8_t		day; 		/**< Day of month, range 1..31 (UTC) */
+	uint8_t		hour; 		/**< Hour of day, range 0..23 (UTC) */
+	uint8_t		min; 		/**< Minute of hour, range 0..59 (UTC) */
+	uint8_t		sec;		/**< Seconds of minute, range 0..60 (UTC) */
+	uint8_t		valid; 		/**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */
+} ubx_payload_rx_nav_timeutc_t;
+
+/* Rx NAV-SVINFO Part 1 */
 typedef struct {
-	uint32_t time_milliseconds; // GPS Millisecond Time of Week
-	int32_t velN; //NED north velocity, cm/s
-	int32_t velE; //NED east velocity, cm/s
-	int32_t velD; //NED down velocity, cm/s
-	uint32_t speed; //Speed (3-D), cm/s
-	uint32_t gSpeed; //Ground Speed (2-D), cm/s
-	int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
-	uint32_t sAcc; //Speed Accuracy Estimate, cm/s
-	uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_velned_packet_t;
-
-//typedef struct {
-//	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
-//	int16_t week; 					/**< Measurement GPS week number */
-//	uint8_t numVis;					/**< Number of visible satellites */
-//
-//	//... rest of package is not used in this implementation
-//
-//} gps_bin_rxm_svsi_packet_t;
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	uint8_t		numCh; 		/**< Number of channels */
+	uint8_t		globalFlags;
+	uint16_t	reserved2;
+} ubx_payload_rx_nav_svinfo_part1_t;
 
+/* Rx NAV-SVINFO Part 2 (repeated) */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_ack_ack_packet_t;
-
+	uint8_t		chn; 		/**< Channel number, 255 for SVs not assigned to a channel */
+	uint8_t		svid; 		/**< Satellite ID */
+	uint8_t		flags;
+	uint8_t		quality;
+	uint8_t		cno;		/**< Carrier to Noise Ratio (Signal Strength) [dbHz] */
+	int8_t		elev; 		/**< Elevation [deg] */
+	int16_t		azim; 		/**< Azimuth [deg] */
+	int32_t		prRes; 		/**< Pseudo range residual [cm] */
+} ubx_payload_rx_nav_svinfo_part2_t;
+
+/* Rx NAV-VELNED */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_ack_nak_packet_t;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	int32_t		velN;		/**< North velocity component [cm/s]*/
+	int32_t		velE;		/**< East velocity component [cm/s]*/
+	int32_t		velD;		/**< Down velocity component [cm/s]*/
+	uint32_t	speed;		/**< Speed (3-D) [cm/s] */
+	uint32_t	gSpeed;		/**< Ground speed (2-D) [cm/s] */
+	int32_t		heading;	/**< Heading of motion 2-D [1e-5 deg] */
+	uint32_t	sAcc;		/**< Speed accuracy estimate [cm/s] */
+	uint32_t	cAcc;		/**< Course / Heading accuracy estimate [1e-5 deg] */
+} ubx_payload_rx_nav_velned_t;
+
+/* Rx MON-HW (ubx6) */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint8_t portID;
-	uint8_t res0;
-	uint16_t res1;
-	uint32_t mode;
-	uint32_t baudRate;
-	uint16_t inProtoMask;
-	uint16_t outProtoMask;
-	uint16_t flags;
-	uint16_t pad;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_prt_packet_t;
-
+	uint32_t	pinSel;
+	uint32_t	pinBank;
+	uint32_t	pinDir;
+	uint32_t	pinVal;
+	uint16_t	noisePerMS;
+	uint16_t	agcCnt;
+	uint8_t		aStatus;
+	uint8_t		aPower;
+	uint8_t		flags;
+	uint8_t		reserved1;
+	uint32_t	usedMask;
+	uint8_t		VP[25];
+	uint8_t		jamInd;
+	uint16_t	reserved3;
+	uint32_t	pinIrq;
+	uint32_t	pullH;
+	uint32_t	pullL;
+} ubx_payload_rx_mon_hw_ubx6_t;
+
+/* Rx MON-HW (ubx7+) */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint16_t measRate;
-	uint16_t navRate;
-	uint16_t timeRef;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_rate_packet_t;
-
+	uint32_t	pinSel;
+	uint32_t	pinBank;
+	uint32_t	pinDir;
+	uint32_t	pinVal;
+	uint16_t	noisePerMS;
+	uint16_t	agcCnt;
+	uint8_t		aStatus;
+	uint8_t		aPower;
+	uint8_t		flags;
+	uint8_t		reserved1;
+	uint32_t	usedMask;
+	uint8_t		VP[17];
+	uint8_t		jamInd;
+	uint16_t	reserved3;
+	uint32_t	pinIrq;
+	uint32_t	pullH;
+	uint32_t	pullL;
+} ubx_payload_rx_mon_hw_ubx7_t;
+
+/* Rx MON-VER Part 1 */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint16_t mask;
-	uint8_t dynModel;
-	uint8_t fixMode;
-	int32_t fixedAlt;
-	uint32_t fixedAltVar;
-	int8_t minElev;
-	uint8_t drLimit;
-	uint16_t pDop;
-	uint16_t tDop;
-	uint16_t pAcc;
-	uint16_t tAcc;
-	uint8_t staticHoldThresh;
-	uint8_t dgpsTimeOut;
-	uint32_t reserved2;
-	uint32_t reserved3;
-	uint32_t reserved4;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_nav5_packet_t;
+	uint8_t		swVersion[30];
+	uint8_t		hwVersion[10];
+} ubx_payload_rx_mon_ver_part1_t;
 
+/* Rx MON-VER Part 2 (repeated) */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint8_t msgClass_payload;
-	uint8_t msgID_payload;
-	uint8_t rate;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_msg_packet_t;
+	uint8_t		extension[30];
+} ubx_payload_rx_mon_ver_part2_t;
+
+/* Rx ACK-ACK */
+typedef	union {
+	uint16_t	msg;
+	struct {
+		uint8_t	clsID;
+		uint8_t	msgID;
+	};
+} ubx_payload_rx_ack_ack_t;
+
+/* Rx ACK-NAK */
+typedef	union {
+	uint16_t	msg;
+	struct {
+		uint8_t	clsID;
+		uint8_t	msgID;
+	};
+} ubx_payload_rx_ack_nak_t;
+
+/* Tx CFG-PRT */
+typedef struct {
+	uint8_t		portID;
+	uint8_t		reserved0;
+	uint16_t	txReady;
+	uint32_t	mode;
+	uint32_t	baudRate;
+	uint16_t	inProtoMask;
+	uint16_t	outProtoMask;
+	uint16_t	flags;
+	uint16_t	reserved5;
+} ubx_payload_tx_cfg_prt_t;
+
+/* Tx CFG-RATE */
+typedef struct {
+	uint16_t	measRate;	/**< Measurement Rate, GPS measurements are taken every measRate milliseconds */
+	uint16_t	navRate;	/**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */
+	uint16_t	timeRef;	/**< Alignment to reference time: 0 = UTC time, 1 = GPS time */
+} ubx_payload_tx_cfg_rate_t;
 
-struct ubx_cfg_msg_rate {
-	uint8_t msg_class;
-	uint8_t msg_id;
+/* Tx CFG-NAV5 */
+typedef struct {
+	uint16_t	mask;
+	uint8_t		dynModel;	/**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
+	uint8_t		fixMode;	/**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */
+	int32_t		fixedAlt;
+	uint32_t	fixedAltVar;
+	int8_t		minElev;
+	uint8_t		drLimit;
+	uint16_t	pDop;
+	uint16_t	tDop;
+	uint16_t	pAcc;
+	uint16_t	tAcc;
+	uint8_t		staticHoldThresh;
+	uint8_t		dgpsTimeOut;
+	uint8_t		cnoThreshNumSVs;	/**< (ubx7+ only, else 0) */
+	uint8_t		cnoThresh;		/**< (ubx7+ only, else 0) */
+	uint16_t	reserved;
+	uint16_t	staticHoldMaxDist;	/**< (ubx8+ only, else 0) */
+	uint8_t		utcStandard;		/**< (ubx8+ only, else 0) */
+	uint8_t		reserved3;
+	uint32_t	reserved4;
+} ubx_payload_tx_cfg_nav5_t;
+
+/* Tx CFG-MSG */
+typedef struct {
+	union {
+		uint16_t	msg;
+		struct {
+			uint8_t	msgClass;
+			uint8_t	msgID;
+		};
+	};
 	uint8_t rate;
-};
+} ubx_payload_tx_cfg_msg_t;
+
+/* General message and payload buffer union */
+typedef union {
+	ubx_payload_rx_nav_pvt_t		payload_rx_nav_pvt;
+	ubx_payload_rx_nav_posllh_t		payload_rx_nav_posllh;
+	ubx_payload_rx_nav_sol_t		payload_rx_nav_sol;
+	ubx_payload_rx_nav_timeutc_t		payload_rx_nav_timeutc;
+	ubx_payload_rx_nav_svinfo_part1_t	payload_rx_nav_svinfo_part1;
+	ubx_payload_rx_nav_svinfo_part2_t	payload_rx_nav_svinfo_part2;
+	ubx_payload_rx_nav_velned_t		payload_rx_nav_velned;
+	ubx_payload_rx_mon_hw_ubx6_t		payload_rx_mon_hw_ubx6;
+	ubx_payload_rx_mon_hw_ubx7_t		payload_rx_mon_hw_ubx7;
+	ubx_payload_rx_mon_ver_part1_t		payload_rx_mon_ver_part1;
+	ubx_payload_rx_mon_ver_part2_t		payload_rx_mon_ver_part2;
+	ubx_payload_rx_ack_ack_t		payload_rx_ack_ack;
+	ubx_payload_rx_ack_nak_t		payload_rx_ack_nak;
+	ubx_payload_tx_cfg_prt_t		payload_tx_cfg_prt;
+	ubx_payload_tx_cfg_rate_t		payload_tx_cfg_rate;
+	ubx_payload_tx_cfg_nav5_t		payload_tx_cfg_nav5;
+	ubx_payload_tx_cfg_msg_t		payload_tx_cfg_msg;
+	uint8_t					raw[];
+} ubx_buf_t;
 
+#pragma pack(pop)
+/*** END OF u-blox protocol binary message and payload definitions ***/
 
-// END the structures of the binary packets
-// ************
-
+/* Decoder state */
 typedef enum {
-	UBX_DECODE_UNINIT = 0,
-	UBX_DECODE_GOT_SYNC1,
-	UBX_DECODE_GOT_SYNC2,
-	UBX_DECODE_GOT_CLASS,
-	UBX_DECODE_GOT_MESSAGEID,
-	UBX_DECODE_GOT_LENGTH1,
-	UBX_DECODE_GOT_LENGTH2
+	UBX_DECODE_SYNC1 = 0,
+	UBX_DECODE_SYNC2,
+	UBX_DECODE_CLASS,
+	UBX_DECODE_ID,
+	UBX_DECODE_LENGTH1,
+	UBX_DECODE_LENGTH2,
+	UBX_DECODE_PAYLOAD,
+	UBX_DECODE_CHKSUM1,
+	UBX_DECODE_CHKSUM2
 } ubx_decode_state_t;
 
-//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
-#pragma pack(pop)
+/* Rx message state */
+typedef enum {
+	UBX_RXMSG_IGNORE = 0,
+	UBX_RXMSG_HANDLE,
+	UBX_RXMSG_DISABLE,
+	UBX_RXMSG_ERROR_LENGTH
+} ubx_rxmsg_state_t;
+
+/* ACK state */
+typedef enum {
+	UBX_ACK_IDLE = 0,
+	UBX_ACK_WAITING,
+	UBX_ACK_GOT_ACK,
+	UBX_ACK_GOT_NAK
+} ubx_ack_state_t;
 
-#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
 
 class UBX : public GPS_Helper
 {
 public:
-	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
+	UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
 	~UBX();
-	int			receive(unsigned timeout);
+	int			receive(const unsigned timeout);
 	int			configure(unsigned &baudrate);
 
 private:
 
 	/**
-	 * Parse the binary MTK packet
+	 * Parse the binary UBX packet
 	 */
-	int			parse_char(uint8_t b);
+	int			parse_char(const uint8_t b);
 
 	/**
-	 * Handle the package once it has arrived
+	 * Start payload rx
 	 */
-	int			handle_message(void);
+	int			payload_rx_init(void);
+
+	/**
+	 * Add payload rx byte
+	 */
+	int			payload_rx_add(const uint8_t b);
+	int			payload_rx_add_nav_svinfo(const uint8_t b);
+	int			payload_rx_add_mon_ver(const uint8_t b);
+
+	/**
+	 * Finish payload rx
+	 */
+	int			payload_rx_done(void);
 
 	/**
 	 * Reset the parse state machine for a fresh start
@@ -349,41 +490,53 @@ private:
 	/**
 	 * While parsing add every byte (except the sync bytes) to the checksum
 	 */
-	void			add_byte_to_checksum(uint8_t);
+	void			add_byte_to_checksum(const uint8_t);
 
 	/**
-	 * Add the two checksum bytes to an outgoing message
+	 * Send a message
 	 */
-	void			add_checksum_to_message(uint8_t *message, const unsigned length);
+	void			send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length);
 
 	/**
-	 * Helper to send a config packet
+	 * Configure message rate
 	 */
-	void			send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
-
-	void			configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
+	void			configure_message_rate(const uint16_t msg, const uint8_t rate);
 
-	void			send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
+	/**
+	 * Calculate & add checksum for given buffer
+	 */
+	void			calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum);
 
-	void			add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+	/**
+	 * Wait for message acknowledge
+	 */
+	int			wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report);
 
-	int			wait_for_ack(unsigned timeout);
+	/**
+	 * Calculate FNV1 hash
+	 */
+	uint32_t		fnv1_32_str(uint8_t *str, uint32_t hval);
 
 	int			_fd;
 	struct vehicle_gps_position_s *_gps_position;
+	struct satellite_info_s *_satellite_info;
+	bool			_enable_sat_info;
 	bool			_configured;
-	bool			_waiting_for_ack;
-	uint8_t			_message_class_needed;
-	uint8_t			_message_id_needed;
+	ubx_ack_state_t		_ack_state;
+	bool			_got_posllh;
+	bool			_got_velned;
 	ubx_decode_state_t	_decode_state;
-	uint8_t			_rx_buffer[RECV_BUFFER_SIZE];
-	unsigned		_rx_count;
+	uint16_t		_rx_msg;
+	ubx_rxmsg_state_t	_rx_state;
+	uint16_t		_rx_payload_length;
+	uint16_t		_rx_payload_index;
 	uint8_t			_rx_ck_a;
 	uint8_t			_rx_ck_b;
-	uint8_t			_message_class;
-	uint8_t			_message_id;
-	unsigned		_payload_size;
-	uint8_t			_disable_cmd_last;
+	hrt_abstime		_disable_cmd_last;
+	uint16_t		_ack_waiting_msg;
+	ubx_buf_t		_buf;
+	uint32_t		_ubx_version;
+	bool			_use_nav_pvt;
 };
 
 #endif /* UBX_H_ */
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 0a047f38f..f17e99e9d 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -122,7 +122,7 @@ private:
 	actuator_controls_s _controls;
 
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	static int	control_callback(uintptr_t handle,
 			uint8_t control_group,
@@ -229,7 +229,7 @@ HIL::init()
 	_task = task_spawn_cmd("fmuhil",
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   2048,
+			   1200,
 			   (main_t)&HIL::task_main_trampoline,
 			   nullptr);
 
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 4c85c0cda..4aef43102 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -71,13 +71,16 @@
 #include <uORB/topics/subsystem_info.h>
 
 #include <float.h>
+#include <getopt.h>
+#include <lib/conversion/rotation.h>
 
 /*
  * HMC5883 internal constants and data structures.
  */
 
 #define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
-#define HMC5883L_DEVICE_PATH		"/dev/hmc5883"
+#define HMC5883L_DEVICE_PATH_INT	"/dev/hmc5883_int"
+#define HMC5883L_DEVICE_PATH_EXT	"/dev/hmc5883_ext"
 
 /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
 #define HMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
@@ -130,7 +133,7 @@ static const int ERROR = -1;
 class HMC5883 : public device::I2C
 {
 public:
-	HMC5883(int bus);
+	HMC5883(int bus, const char *path, enum Rotation rotation);
 	virtual ~HMC5883();
 
 	virtual int		init();
@@ -158,16 +161,25 @@ private:
 	int			_class_instance;
 
 	orb_advert_t		_mag_topic;
+	orb_advert_t		_subsystem_pub;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
+	perf_counter_t		_range_errors;
+	perf_counter_t		_conf_errors;
 
 	/* status reporting */
 	bool			_sensor_ok;		/**< sensor was found and reports ok */
 	bool			_calibrated;		/**< the calibration is valid */
 
 	int			_bus;			/**< the bus the device is connected to */
+	enum Rotation		_rotation;
+
+	struct mag_report	_last_report;           /**< used for info() */
+
+	uint8_t			_range_bits;
+	uint8_t			_conf_reg;
 
 	/**
 	 * Test whether the device supported by the driver is present at a
@@ -227,6 +239,23 @@ private:
 	int 			set_range(unsigned range);
 
 	/**
+	 * check the sensor range.
+	 *
+	 * checks that the range of the sensor is correctly set, to
+	 * cope with communication errors causing the range to change
+	 */
+	void 			check_range(void);
+
+	/**
+	 * check the sensor configuration.
+	 *
+	 * checks that the config of the sensor is correctly set, to
+	 * cope with communication errors causing the configuration to
+	 * change
+	 */
+	void 			check_conf(void);
+
+	/**
 	 * Perform a poll cycle; collect from the previous measurement
 	 * and start a new one.
 	 *
@@ -316,21 +345,31 @@ private:
 extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 
 
-HMC5883::HMC5883(int bus) :
-	I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
+HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
+	I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
 	_measure_ticks(0),
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
-	_mag_topic(-1),
+	_collect_phase(false),
 	_class_instance(-1),
+	_mag_topic(-1),
+	_subsystem_pub(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
+	_range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")),
+	_conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")),
 	_sensor_ok(false),
 	_calibrated(false),
-	_bus(bus)
+	_bus(bus),
+	_rotation(rotation),
+	_last_report{0},
+	_range_bits(0),
+	_conf_reg(0)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+
 	// enable debug() calls
 	_debug_enabled = false;
 
@@ -361,6 +400,8 @@ HMC5883::~HMC5883()
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
 	perf_free(_buffer_overflows);
+	perf_free(_range_errors);
+	perf_free(_conf_errors);
 }
 
 int
@@ -391,45 +432,43 @@ out:
 
 int HMC5883::set_range(unsigned range)
 {
-	uint8_t range_bits;
-
 	if (range < 1) {
-		range_bits = 0x00;
+		_range_bits = 0x00;
 		_range_scale = 1.0f / 1370.0f;
 		_range_ga = 0.88f;
 
 	} else if (range <= 1) {
-		range_bits = 0x01;
+		_range_bits = 0x01;
 		_range_scale = 1.0f / 1090.0f;
 		_range_ga = 1.3f;
 
 	} else if (range <= 2) {
-		range_bits = 0x02;
+		_range_bits = 0x02;
 		_range_scale = 1.0f / 820.0f;
 		_range_ga = 1.9f;
 
 	} else if (range <= 3) {
-		range_bits = 0x03;
+		_range_bits = 0x03;
 		_range_scale = 1.0f / 660.0f;
 		_range_ga = 2.5f;
 
 	} else if (range <= 4) {
-		range_bits = 0x04;
+		_range_bits = 0x04;
 		_range_scale = 1.0f / 440.0f;
 		_range_ga = 4.0f;
 
 	} else if (range <= 4.7f) {
-		range_bits = 0x05;
+		_range_bits = 0x05;
 		_range_scale = 1.0f / 390.0f;
 		_range_ga = 4.7f;
 
 	} else if (range <= 5.6f) {
-		range_bits = 0x06;
+		_range_bits = 0x06;
 		_range_scale = 1.0f / 330.0f;
 		_range_ga = 5.6f;
 
 	} else {
-		range_bits = 0x07;
+		_range_bits = 0x07;
 		_range_scale = 1.0f / 230.0f;
 		_range_ga = 8.1f;
 	}
@@ -439,7 +478,7 @@ int HMC5883::set_range(unsigned range)
 	/*
 	 * Send the command to set the range
 	 */
-	ret = write_reg(ADDR_CONF_B, (range_bits << 5));
+	ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
 
 	if (OK != ret)
 		perf_count(_comms_errors);
@@ -450,7 +489,53 @@ int HMC5883::set_range(unsigned range)
 	if (OK != ret)
 		perf_count(_comms_errors);
 
-	return !(range_bits_in == (range_bits << 5));
+	return !(range_bits_in == (_range_bits << 5));
+}
+
+/**
+   check that the range register has the right value. This is done
+   periodically to cope with I2C bus noise causing the range of the
+   compass changing.
+ */
+void HMC5883::check_range(void)
+{
+	int ret;
+
+	uint8_t range_bits_in;
+	ret = read_reg(ADDR_CONF_B, range_bits_in);
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		return;
+	}
+	if (range_bits_in != (_range_bits<<5)) {
+		perf_count(_range_errors);
+		ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
+		if (OK != ret)
+			perf_count(_comms_errors);
+	}
+}
+
+/**
+   check that the configuration register has the right value. This is
+   done periodically to cope with I2C bus noise causing the
+   configuration of the compass to change.
+ */
+void HMC5883::check_conf(void)
+{
+	int ret;
+
+	uint8_t conf_reg_in;
+	ret = read_reg(ADDR_CONF_A, conf_reg_in);
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		return;
+	}
+	if (conf_reg_in != _conf_reg) {
+		perf_count(_conf_errors);
+		ret = write_reg(ADDR_CONF_A, _conf_reg);
+		if (OK != ret)
+			perf_count(_comms_errors);
+	}
 }
 
 int
@@ -713,7 +798,7 @@ HMC5883::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			debug("collection error");
 			/* restart the measurement state machine */
 			start();
 			return;
@@ -740,7 +825,7 @@ HMC5883::cycle()
 
 	/* measurement phase */
 	if (OK != measure())
-		log("measure error");
+		debug("measure error");
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -784,7 +869,7 @@ HMC5883::collect()
 	} report;
 	int	ret = -EIO;
 	uint8_t	cmd;
-
+	uint8_t check_counter;
 
 	perf_begin(_sample_perf);
 	struct mag_report new_report;
@@ -857,6 +942,9 @@ HMC5883::collect()
 	/* z remains z */
 	new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 
+	// apply user specified rotation
+	rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
+
 	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
 
 		if (_mag_topic != -1) {
@@ -870,6 +958,8 @@ HMC5883::collect()
 		}
 	}
 
+	_last_report = new_report;
+
 	/* post a report to the ring */
 	if (_reports->force(&new_report)) {
 		perf_count(_buffer_overflows);
@@ -878,6 +968,21 @@ HMC5883::collect()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
+	/*
+	  periodically check the range register and configuration
+	  registers. With a bad I2C cable it is possible for the
+	  registers to become corrupt, leading to bad readings. It
+	  doesn't happen often, but given the poor cables some
+	  vehicles have it is worth checking for.
+	 */
+	check_counter = perf_event_count(_sample_perf) % 256;
+	if (check_counter == 0) {
+		check_range();
+	}
+	if (check_counter == 128) {
+		check_conf();
+	}
+
 	ret = OK;
 
 out:
@@ -1042,31 +1147,28 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
 
-	/* set back to normal mode */
-	/* Set to 1.1 Gauss */
-	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
-		goto out;
-	}
-
-	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
-		warnx("failed to disable sensor calibration mode");
-		goto out;
-	}
-
 	/* set scaling in device */
 	mscale_previous.x_scale = scaling[0];
 	mscale_previous.y_scale = scaling[1];
 	mscale_previous.z_scale = scaling[2];
 
+	ret = OK;
+
+out:
+
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
 		warn("WARNING: failed to set new scale / offsets for mag");
-		goto out;
 	}
 
-	ret = OK;
+	/* set back to normal mode */
+	/* Set to 1.1 Gauss */
+	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
+		warnx("failed to set 1.1 Ga range");
+	}
 
-out:
+	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
+		warnx("failed to disable sensor calibration mode");
+	}
 
 	if (ret == OK) {
 		if (!check_scale()) {
@@ -1136,13 +1238,12 @@ int HMC5883::check_calibration()
 			true,
 			_calibrated,
 			SUBSYSTEM_TYPE_MAG};
-		static orb_advert_t pub = -1;
 
 		if (!(_pub_blocked)) {
-			if (pub > 0) {
-				orb_publish(ORB_ID(subsystem_info), pub, &info);
+			if (_subsystem_pub > 0) {
+				orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
 			} else {
-				pub = orb_advertise(ORB_ID(subsystem_info), &info);
+				_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
 			}
 		}
 	}
@@ -1155,25 +1256,24 @@ int HMC5883::set_excitement(unsigned enable)
 {
 	int ret;
 	/* arm the excitement strap */
-	uint8_t conf_reg;
-	ret = read_reg(ADDR_CONF_A, conf_reg);
+	ret = read_reg(ADDR_CONF_A, _conf_reg);
 
 	if (OK != ret)
 		perf_count(_comms_errors);
 
 	if (((int)enable) < 0) {
-		conf_reg |= 0x01;
+		_conf_reg |= 0x01;
 
 	} else if (enable > 0) {
-		conf_reg |= 0x02;
+		_conf_reg |= 0x02;
 
 	} else {
-		conf_reg &= ~0x03;
+		_conf_reg &= ~0x03;
 	}
 
-        // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
+        // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
 
-	ret = write_reg(ADDR_CONF_A, conf_reg);
+	ret = write_reg(ADDR_CONF_A, _conf_reg);
 
 	if (OK != ret)
 		perf_count(_comms_errors);
@@ -1183,7 +1283,7 @@ int HMC5883::set_excitement(unsigned enable)
 
 	//print_info();
 
-	return !(conf_reg == conf_reg_ret);
+	return !(_conf_reg == conf_reg_ret);
 }
 
 int
@@ -1221,10 +1321,11 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("output  (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
 	printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
 	printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", 
 	       (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
-	       (double)1.0/_range_scale, (double)_range_ga);
+	       (double)(1.0f/_range_scale), (double)_range_ga);
 	_reports->print_info("report queue");
 }
 
@@ -1240,63 +1341,87 @@ namespace hmc5883
 #endif
 const int ERROR = -1;
 
-HMC5883	*g_dev;
+HMC5883	*g_dev_int;
+HMC5883	*g_dev_ext;
 
-void	start();
-void	test();
-void	reset();
-void	info();
-int	calibrate();
+void	start(int bus, enum Rotation rotation);
+void	test(int bus);
+void	reset(int bus);
+void	info(int bus);
+int	calibrate(int bus);
+void	usage();
 
 /**
  * Start the driver.
  */
 void
-start()
+start(int bus, enum Rotation rotation)
 {
 	int fd;
 
-	if (g_dev != nullptr)
-		/* if already started, the still command succeeded */
-		errx(0, "already started");
-
 	/* create the driver, attempt expansion bus first */
-	g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
-	if (g_dev != nullptr && OK != g_dev->init()) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
+		if (g_dev_ext != nullptr)
+			errx(0, "already started external");
+		g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation);
+		if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
+			delete g_dev_ext;
+			g_dev_ext = nullptr;
+		}
 	}
 			
 
 #ifdef PX4_I2C_BUS_ONBOARD
 	/* if this failed, attempt onboard sensor */
-	if (g_dev == nullptr) {
-		g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
-		if (g_dev != nullptr && OK != g_dev->init()) {
+	if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
+		if (g_dev_int != nullptr)
+			errx(0, "already started internal");
+		g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
+		if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+			if (bus == PX4_I2C_BUS_ONBOARD) {
+				goto fail;
+			}
+		}
+		if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
 			goto fail;
 		}
 	}
 #endif
 
-	if (g_dev == nullptr)
+	if (g_dev_int == nullptr && g_dev_ext == nullptr)
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+	if (g_dev_int != nullptr) {
+		fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY);
+		if (fd < 0)
+			goto fail;
 
-	if (fd < 0)
-		goto fail;
+		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+			goto fail;
+		close(fd);
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		goto fail;
+	if (g_dev_ext != nullptr) {
+		fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY);
+		if (fd < 0)
+			goto fail;
+
+		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+			goto fail;
+		close(fd);
+	}
 
 	exit(0);
 
 fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
+		delete g_dev_int;
+		g_dev_int = nullptr;
+	}
+	if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
+		delete g_dev_ext;
+		g_dev_ext = nullptr;
 	}
 
 	errx(1, "driver start failed");
@@ -1308,16 +1433,17 @@ fail:
  * and automatic modes.
  */
 void
-test()
+test(int bus)
 {
 	struct mag_report report;
 	ssize_t sz;
 	int ret;
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
 
-	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -1410,14 +1536,15 @@ test()
  * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
  * Using the self test method described above, the user can scale sensor
  */
-int calibrate()
+int calibrate(int bus)
 {
 	int ret;
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
 
-	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
 
 	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
 		warnx("failed to enable sensor calibration mode");
@@ -1437,9 +1564,11 @@ int calibrate()
  * Reset the driver.
  */
 void
-reset()
+reset(int bus)
 {
-	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
+
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1457,8 +1586,9 @@ reset()
  * Print a little info about the driver.
  */
 void
-info()
+info(int bus)
 {
+	HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
 	if (g_dev == nullptr)
 		errx(1, "driver not running");
 
@@ -1468,40 +1598,91 @@ info()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'");
+	warnx("options:");
+	warnx("    -R rotation");
+	warnx("    -C calibrate on start");
+	warnx("    -X only external bus");
+#ifdef PX4_I2C_BUS_ONBOARD
+	warnx("    -I only internal bus");
+#endif
+}
+
 } // namespace
 
 int
 hmc5883_main(int argc, char *argv[])
 {
+	int ch;
+	int bus = -1;
+	enum Rotation rotation = ROTATION_NONE;
+        bool calibrate = false;
+
+	while ((ch = getopt(argc, argv, "XIR:C")) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+#ifdef PX4_I2C_BUS_ONBOARD
+		case 'I':
+			bus = PX4_I2C_BUS_ONBOARD;
+			break;
+#endif
+		case 'X':
+			bus = PX4_I2C_BUS_EXPANSION;
+			break;
+		case 'C':
+			calibrate = true;
+			break;
+		default:
+			hmc5883::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];	
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
-		hmc5883::start();
+	if (!strcmp(verb, "start")) {
+		hmc5883::start(bus, rotation);
+		if (calibrate) {
+			if (hmc5883::calibrate(bus) == 0) {
+				errx(0, "calibration successful");
+				
+			} else {
+				errx(1, "calibration failed");
+			}
+		}
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
-		hmc5883::test();
+	if (!strcmp(verb, "test"))
+		hmc5883::test(bus);
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
-		hmc5883::reset();
+	if (!strcmp(verb, "reset"))
+		hmc5883::reset(bus);
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
-		hmc5883::info();
+	if (!strcmp(verb, "info") || !strcmp(verb, "status"))
+		hmc5883::info(bus);
 
 	/*
 	 * Autocalibrate the scaling
 	 */
-	if (!strcmp(argv[1], "calibrate")) {
-		if (hmc5883::calibrate() == 0) {
+	if (!strcmp(verb, "calibrate")) {
+		if (hmc5883::calibrate(bus) == 0) {
 			errx(0, "calibration successful");
 
 		} else {
diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk
index b5f5762ba..47aea6caf 100644
--- a/src/drivers/hott/hott_sensors/module.mk
+++ b/src/drivers/hott/hott_sensors/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= hott_sensors
 SRCS		= hott_sensors.cpp \
 		  ../messages.cpp \
 		  ../comms.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk
index b19cbd14c..cd7bdbc85 100644
--- a/src/drivers/hott/hott_telemetry/module.mk
+++ b/src/drivers/hott/hott_telemetry/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= hott_telemetry
 SRCS		= hott_telemetry.cpp \
 		  ../messages.cpp \
 		  ../comms.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index bb8d45bea..086132573 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -51,6 +51,8 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
+#include <drivers/drv_hrt.h>
+
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5
 
@@ -62,7 +64,6 @@ static int _airspeed_sub = -1;
 static int _esc_sub = -1;
 
 static orb_advert_t _esc_pub;
-struct esc_status_s _esc;
 
 static bool _home_position_set = false;
 static double _home_lat = 0.0d;
@@ -82,8 +83,6 @@ init_sub_messages(void)
 void 
 init_pub_messages(void)
 {
-	memset(&_esc, 0, sizeof(_esc));
-	_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
 }
 
 void
@@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer)
 	size_t size = sizeof(msg);
 	memset(&msg, 0, size);
 	memcpy(&msg, buffer, size);
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+
+	// Publish it.
+	esc.timestamp = hrt_absolute_time();
+	esc.esc_count = 1;
+	esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
+
+	esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
+	esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
+	esc.esc[0].esc_temperature = msg.temperature1 - 20; 
+	esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
+	esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
 
 	/* announce the esc if needed, just publish else */
 	if (_esc_pub > 0) {
-		orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
+		orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
 	} else {
-		_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
+		_esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
 	}
-
-	// Publish it.
-	_esc.esc_count = 1;
-	_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
-
-	_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
-	_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
-	_esc.esc[0].esc_temperature = msg.temperature1 - 20; 
-	_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
-	_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
 }
 
 void 
@@ -224,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 	msg.sensor_id = GPS_SENSOR_ID;
 	msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
 
-	msg.gps_num_sat = gps.satellites_visible;
+	msg.gps_num_sat = gps.satellites_used;
 
 	/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
 	msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
@@ -295,8 +297,8 @@ build_gps_response(uint8_t *buffer, size_t *size)
 			memset(&home, 0, sizeof(home));
 			orb_copy(ORB_ID(home_position), _home_sub, &home);
 
-			_home_lat = ((double)(home.lat))*1e-7d;
-			_home_lon = ((double)(home.lon))*1e-7d;
+			_home_lat = home.lat;
+			_home_lon = home.lon;
 			_home_position_set = true;
 		}
 
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 90c3db9ae..64d1a7e55 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -34,6 +34,9 @@
 /**
  * @file l3gd20.cpp
  * Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ *
+ * Note: With the exception of the self-test feature, the ST L3G4200D is
+ *       also supported by this driver.
  */
 
 #include <nuttx/config.h>
@@ -51,6 +54,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -65,6 +69,7 @@
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
 
 #define L3GD20_DEVICE_PATH "/dev/l3gd20"
 
@@ -89,9 +94,11 @@ static const int ERROR = -1;
 #define ADDR_WHO_AM_I			0x0F
 #define WHO_I_AM_H 				0xD7
 #define WHO_I_AM				0xD4
+#define WHO_I_AM_L3G4200D		0xD3	/* for L3G4200D */
 
 #define ADDR_CTRL_REG1			0x20
 #define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
+
 /* keep lowpass low to avoid noise issues */
 #define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
 #define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (0<<5) | (1<<4))
@@ -166,15 +173,20 @@ static const int ERROR = -1;
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
 #define L3GD20_DEFAULT_RATE			760
+#define L3G4200D_DEFAULT_RATE			800
 #define L3GD20_DEFAULT_RANGE_DPS		2000
 #define L3GD20_DEFAULT_FILTER_FREQ		30
 
+#ifndef SENSOR_BOARD_ROTATION_DEFAULT
+#define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
+#endif
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
 {
 public:
-	L3GD20(int bus, const char* path, spi_dev_e device);
+	L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
 	virtual ~L3GD20();
 
 	virtual int		init();
@@ -216,6 +228,11 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	/* true if an L3G4200D is detected */
+	bool	_is_l3g4200d;
+
+	enum Rotation		_rotation;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -315,7 +332,7 @@ private:
 	 int 			self_test();
 };
 
-L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
+L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
 	_call_interval(0),
 	_reports(nullptr),
@@ -324,14 +341,16 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_topic(-1),
 	_class_instance(-1),
 	_current_rate(0),
-	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
+	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
 	_read(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
 	_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
 	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
-	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
+	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_is_l3g4200d(false),
+        _rotation(rotation)                                            
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -413,14 +432,7 @@ L3GD20::probe()
 	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
 
-		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
-		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
-			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
-		#else
-			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
-		#endif
-
+		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
 		success = true;
 	}
 
@@ -430,6 +442,13 @@ L3GD20::probe()
 		success = true;
 	}
 
+	/* Detect the L3G4200D used on AeroCore */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
+		_is_l3g4200d = true;
+		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
+		success = true;
+	}
+
 	if (success)
 		return OK;
 
@@ -502,6 +521,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
+				if (_is_l3g4200d) {
+					return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
+				}
 				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
@@ -683,23 +705,26 @@ L3GD20::set_samplerate(unsigned frequency)
 	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
 
 	if (frequency == 0)
-		frequency = 760;
+		frequency = _is_l3g4200d ? 800 : 760;
 
-	/* use limits good for H or non-H models */
+	/*
+	 * Use limits good for H or non-H models. Rates are slightly different
+	 * for L3G4200D part but register settings are the same.
+	 */
 	if (frequency <= 100) {
-		_current_rate = 95;
+		_current_rate = _is_l3g4200d ? 100 : 95;
 		bits |= RATE_95HZ_LP_25HZ;
 
 	} else if (frequency <= 200) {
-		_current_rate = 190;
+		_current_rate = _is_l3g4200d ? 200 : 190;
 		bits |= RATE_190HZ_LP_50HZ;
 
 	} else if (frequency <= 400) {
-		_current_rate = 380;
+		_current_rate = _is_l3g4200d ? 400 : 380;
 		bits |= RATE_380HZ_LP_50HZ;
 
 	} else if (frequency <= 800) {
-		_current_rate = 760;
+		_current_rate = _is_l3g4200d ? 800 : 760;
 		bits |= RATE_760HZ_LP_50HZ;
 	} else {
 		return -EINVAL;
@@ -772,7 +797,7 @@ L3GD20::reset()
 	 * callback fast enough to not miss data. */
 	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
 
-	set_samplerate(0); // 760Hz
+	set_samplerate(0); // 760Hz or 800Hz
 	set_range(L3GD20_DEFAULT_RANGE_DPS);
 	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
 
@@ -801,7 +826,7 @@ L3GD20::measure()
 	// if the gyro doesn't have any data ready then re-schedule
 	// for 100 microseconds later. This ensures we don't double
 	// read a value and then miss the next value
-	if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
+	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
 		perf_count(_reschedules);
 		hrt_call_delay(&_call, 100);
 		return;
@@ -894,6 +919,9 @@ L3GD20::measure()
 	report.y = _gyro_filter_y.apply(report.y);
 	report.z = _gyro_filter_z.apply(report.z);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, report.x, report.y, report.z);
+
 	report.scaling = _gyro_range_scale;
 	report.range_rad_s = _gyro_range_rad_s;
 
@@ -954,7 +982,8 @@ namespace l3gd20
 
 L3GD20	*g_dev;
 
-void	start();
+void	usage();
+void	start(bool external_bus, enum Rotation rotation);
 void	test();
 void	reset();
 void	info();
@@ -963,7 +992,7 @@ void	info();
  * Start the driver.
  */
 void
-start()
+start(bool external_bus, enum Rotation rotation)
 {
 	int fd;
 
@@ -971,7 +1000,15 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+        if (external_bus) {
+#ifdef PX4_SPI_BUS_EXT
+		g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation);
+#else
+		errx(0, "External SPI not available");
+#endif
+	} else {
+		g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation);
+	}
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -1083,35 +1120,64 @@ info()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R rotation");
+}
 
 } // namespace
 
 int
 l3gd20_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+		default:
+			l3gd20::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(argv[1], "start"))
-		l3gd20::start();
+	if (!strcmp(verb, "start"))
+		l3gd20::start(external_bus, rotation);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(verb, "test"))
 		l3gd20::test();
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(verb, "reset"))
 		l3gd20::reset();
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(verb, "info"))
 		l3gd20::info();
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp
new file mode 100644
index 000000000..a69e6ee55
--- /dev/null
+++ b/src/drivers/ll40ls/ll40ls.cpp
@@ -0,0 +1,882 @@
+/****************************************************************************
+ *
+ *   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 ll40ls.cpp
+ * @author Allyson Kreft
+ *
+ * Driver for the PulsedLight Lidar-Lite range finders connected via I2C.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define LL40LS_BUS 			PX4_I2C_BUS_EXPANSION
+#define LL40LS_BASEADDR 	0x42 /* 7-bit address */
+#define LL40LS_DEVICE_PATH 	"/dev/ll40ls"
+
+/* LL40LS Registers addresses */
+
+#define LL40LS_MEASURE_REG		0x00		/* Measure range register */
+#define LL40LS_MSRREG_ACQUIRE	0x04		/* Value to initiate a measurement, varies based on sensor revision */
+#define LL40LS_DISTHIGH_REG		0x8F		/* High byte of distance register, auto increment */
+
+/* Device limits */
+#define LL40LS_MIN_DISTANCE (0.00f)
+#define LL40LS_MAX_DISTANCE (14.00f)
+
+#define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class LL40LS : public device::I2C
+{
+public:
+	LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR);
+	virtual ~LL40LS();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	int					_class_instance;
+
+	orb_advert_t		_range_finder_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE
+	* and LL40LS_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+
+
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
+
+LL40LS::LL40LS(int bus, int address) :
+	I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000),
+	_min_distance(LL40LS_MIN_DISTANCE),
+	_max_distance(LL40LS_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_class_instance(-1),
+	_range_finder_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows"))
+{
+	// up the retries since the device misses the first measure attempts
+	I2C::_retries = 3;
+
+	// enable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+LL40LS::~LL40LS()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+	
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
+	}
+	
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
+}
+
+int
+LL40LS::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		goto out;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
+
+	if (_reports == nullptr) {
+		goto out;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {	
+		/* get a publish handle on the range finder topic */
+		struct range_finder_report rf_report;
+		measure();
+		_reports->get(&rf_report);
+		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+
+		if (_range_finder_topic < 0) {
+			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+		}
+	}
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+LL40LS::probe()
+{
+	return measure();
+}
+
+void
+LL40LS::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+LL40LS::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+LL40LS::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+LL40LS::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+			/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
+						return -EINVAL;
+					}
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0) {
+			return SENSOR_POLLRATE_MANUAL;
+		}
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+
+			irqrestore(flags);
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _reports->size();
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
+			set_minimum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
+			set_maximum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+LL40LS::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(LL40LS_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+LL40LS::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
+	ret = transfer(cmd, sizeof(cmd), nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+LL40LS::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+
+	perf_begin(_sample_perf);
+
+	// read the high and low byte distance registers
+	uint8_t distance_reg = LL40LS_DISTHIGH_REG;
+	ret = transfer(&distance_reg, 1, &val[0], sizeof(val));
+
+	if (ret < 0) {
+		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint16_t distance = (val[0] << 8) | val[1];
+	float si_units = distance * 0.01f; /* cm to m */
+	struct range_finder_report report;
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	report.timestamp = hrt_absolute_time();
+	report.error_count = perf_event_count(_comms_errors);
+	report.distance = si_units;
+	if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
+		report.valid = 1;
+	}
+	else {
+		report.valid = 0;
+	}
+
+	/* publish it, if we are the primary */
+	if (_range_finder_topic >= 0) {
+		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+	}
+
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+LL40LS::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&LL40LS::cycle_trampoline, this, 1);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_RANGEFINDER
+	};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+LL40LS::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+LL40LS::cycle_trampoline(void *arg)
+{
+	LL40LS *dev = (LL40LS *)arg;
+
+	dev->cycle();
+}
+
+void
+LL40LS::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&LL40LS::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure()) {
+		log("measure error");
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&LL40LS::cycle_trampoline,
+		   this,
+		   USEC2TICK(LL40LS_CONVERSION_INTERVAL));
+}
+
+void
+LL40LS::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ll40ls
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+LL40LS	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new LL40LS(LL40LS_BUS);
+
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		goto fail;
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		goto fail;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		goto fail;
+	}
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		errx(1, "driver not running");
+	}
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH);
+	}
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		err(1, "immediate read failed");
+	}
+
+	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
+		errx(1, "failed to set 2Hz poll rate");
+	}
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			errx(1, "timed out waiting for sensor data");
+		}
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			err(1, "periodic read failed");
+		}
+
+		warnx("periodic read %u", i);
+		warnx("measurement: %0.3f", (double)report.distance);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "failed ");
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		err(1, "driver reset failed");
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "driver poll restart failed");
+	}
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+ll40ls_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		ll40ls::start();
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		ll40ls::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		ll40ls::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		ll40ls::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		ll40ls::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/modules/fixedwing_pos_control/module.mk b/src/drivers/ll40ls/module.mk
index b976377e9..fb627afee 100644
--- a/src/modules/fixedwing_pos_control/module.mk
+++ b/src/drivers/ll40ls/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   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
@@ -32,9 +32,11 @@
 ############################################################################
 
 #
-# Fixedwing PositionControl application
+# Makefile to build the PulsedLight Lidar-Lite driver.
 #
 
-MODULE_COMMAND	= fixedwing_pos_control
+MODULE_COMMAND	= ll40ls
 
-SRCS		= fixedwing_pos_control_main.c
+SRCS		= ll40ls.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 4dee7649b..45e775055 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -52,6 +52,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -68,6 +69,7 @@
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -75,12 +77,17 @@
 #endif
 static const int ERROR = -1;
 
+// enable this to debug the buggy lsm303d sensor in very early
+// prototype pixhawk boards
+#define CHECK_EXTREMES 0
+
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
 #define ADDR_INCREMENT			(1<<6)
 
 #define LSM303D_DEVICE_PATH_ACCEL	"/dev/lsm303d_accel"
+#define LSM303D_DEVICE_PATH_ACCEL_EXT	"/dev/lsm303d_accel_ext"
 #define LSM303D_DEVICE_PATH_MAG		"/dev/lsm303d_mag"
 
 /* register addresses: A: accel, M: mag, T: temp */
@@ -216,7 +223,7 @@ class LSM303D_mag;
 class LSM303D : public device::SPI
 {
 public:
-	LSM303D(int bus, const char* path, spi_dev_e device);
+	LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
 	virtual ~LSM303D();
 
 	virtual int		init();
@@ -305,7 +312,8 @@ private:
 	uint64_t		_last_log_us;	
 	uint64_t		_last_log_sync_us;	
 	uint64_t		_last_log_reg_us;	
-	uint64_t		_last_log_alarm_us;	
+	uint64_t		_last_log_alarm_us;
+	enum Rotation		_rotation;
 
 	/**
 	 * Start automatic measurement.
@@ -485,7 +493,7 @@ private:
 };
 
 
-LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
 	_mag(new LSM303D_mag(this)),
 	_call_accel_interval(0),
@@ -519,8 +527,11 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_last_log_us(0),
 	_last_log_sync_us(0),
 	_last_log_reg_us(0),
-	_last_log_alarm_us(0)
+	_last_log_alarm_us(0),
+	_rotation(rotation)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
 	// enable debug() calls
 	_debug_enabled = true;
 
@@ -830,7 +841,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 		 */
 		while (count--) {
 			if (_accel_reports->get(arb)) {
+#if CHECK_EXTREMES
 				check_extremes(arb);
+#endif
 				ret += sizeof(*arb);
 				arb++;
 			}
@@ -880,7 +893,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 
 	/* manual measurement */
 	_mag_reports->flush();
-	measure();
+	_mag->measure();
 
 	/* measurement will have generated a report, copy it out */
 	if (_mag_reports->get(mrb))
@@ -1533,6 +1546,9 @@ LSM303D::measure()
 	accel_report.y = _accel_filter_y.apply(y_in_new);
 	accel_report.z = _accel_filter_z.apply(z_in_new);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z);
+
 	accel_report.scaling = _accel_range_scale;
 	accel_report.range_m_s2 = _accel_range_m_s2;
 
@@ -1609,6 +1625,9 @@ LSM303D::mag_measure()
 	mag_report.scaling = _mag_range_scale;
 	mag_report.range_ga = (float)_mag_range_ga;
 
+	// apply user specified rotation
+	rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z);
+
 	_mag_reports->force(&mag_report);
 
 	/* XXX please check this poll_notify, is it the right one? */
@@ -1774,26 +1793,34 @@ namespace lsm303d
 
 LSM303D	*g_dev;
 
-void	start();
+void	start(bool external_bus, enum Rotation rotation);
 void	test();
 void	reset();
 void	info();
 void	regdump();
 void	logging();
+void	usage();
 
 /**
  * Start the driver.
  */
 void
-start()
+start(bool external_bus, enum Rotation rotation)
 {
 	int fd, fd_mag;
-
 	if (g_dev != nullptr)
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+        if (external_bus) {
+        	#ifdef PX4_SPI_BUS_EXT
+		g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
+		#else
+		errx(0, "External SPI not available");
+		#endif
+	} else {
+		g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation);
+	}
 
 	if (g_dev == nullptr) {
 		warnx("failed instantiating LSM303D obj");
@@ -1989,47 +2016,76 @@ logging()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R rotation");
+}
 
 } // namespace
 
 int
 lsm303d_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+		default:
+			lsm303d::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(argv[1], "start"))
-		lsm303d::start();
+	if (!strcmp(verb, "start"))
+		lsm303d::start(external_bus, rotation);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(verb, "test"))
 		lsm303d::test();
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(verb, "reset"))
 		lsm303d::reset();
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(verb, "info"))
 		lsm303d::info();
 
 	/*
 	 * dump device registers
 	 */
-	if (!strcmp(argv[1], "regdump"))
+	if (!strcmp(verb, "regdump"))
 		lsm303d::regdump();
 
 	/*
 	 * dump device registers
 	 */
-	if (!strcmp(argv[1], "logging"))
+	if (!strcmp(verb, "logging"))
 		lsm303d::logging();
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index c910e2890..beb6c8e35 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -37,7 +37,7 @@
  *
  * Driver for the Maxbotix sonar range finders connected via I2C.
  */
-	 
+
 #include <nuttx/config.h>
 
 #include <drivers/device/i2c.h>
@@ -74,6 +74,7 @@
 /* Configuration Constants */
 #define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
 #define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+#define MB12XX_DEVICE_PATH	"/dev/mb12xx"
 
 /* MB12xx Registers addresses */
 
@@ -84,7 +85,7 @@
 /* Device limits */
 #define MB12XX_MIN_DISTANCE (0.20f)
 #define MB12XX_MAX_DISTANCE (7.65f)
-	 
+
 #define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
 
 /* oddly, ERROR is not defined for c++ */
@@ -102,17 +103,17 @@ class MB12XX : public device::I2C
 public:
 	MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
 	virtual ~MB12XX();
-	
+
 	virtual int 		init();
-	
+
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
-	
+
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
 	void				print_info();
-	
+
 protected:
 	virtual int			probe();
 
@@ -124,13 +125,14 @@ private:
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
-	
+	int					_class_instance;
+
 	orb_advert_t		_range_finder_topic;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
-	
+
 	/**
 	* Test whether the device supported by the driver is present at a
 	* specific address.
@@ -139,7 +141,7 @@ private:
 	* @return		True if the device is present.
 	*/
 	int					probe_address(uint8_t address);
-	
+
 	/**
 	* Initialise the automatic measurement state machine and start it.
 	*
@@ -147,12 +149,12 @@ private:
 	*       to make it more aggressive about resetting the bus in case of errors.
 	*/
 	void				start();
-	
+
 	/**
 	* Stop the automatic measurement state machine.
 	*/
 	void				stop();
-	
+
 	/**
 	* Set the min and max distance thresholds if you want the end points of the sensors
 	* range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE
@@ -162,7 +164,7 @@ private:
 	void				set_maximum_distance(float max);
 	float				get_minimum_distance();
 	float				get_maximum_distance();
-	
+
 	/**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
@@ -177,8 +179,8 @@ private:
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
 	static void		cycle_trampoline(void *arg);
-	
-	
+
+
 };
 
 /*
@@ -187,21 +189,22 @@ private:
 extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]);
 
 MB12XX::MB12XX(int bus, int address) :
-	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
+	I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000),
 	_min_distance(MB12XX_MIN_DISTANCE),
 	_max_distance(MB12XX_MAX_DISTANCE),
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
 	_collect_phase(false),
+	_class_instance(-1),
 	_range_finder_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
-	
+	_debug_enabled = false;
+
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
 }
@@ -212,8 +215,18 @@ MB12XX::~MB12XX()
 	stop();
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
+	}
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
 }
 
 int
@@ -222,22 +235,30 @@ MB12XX::init()
 	int ret = ERROR;
 
 	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	if (I2C::init() != OK) {
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
 		goto out;
+	}
 
-	/* get a publish handle on the range finder topic */
-	struct range_finder_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
+	_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
 
-	if (_range_finder_topic < 0)
-		debug("failed to create sensor_range_finder object. Did you start uOrb?");
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* get a publish handle on the range finder topic */
+		struct range_finder_report rf_report;
+		measure();
+		_reports->get(&rf_report);
+		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+
+		if (_range_finder_topic < 0) {
+			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+		}
+	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
@@ -256,13 +277,13 @@ void
 MB12XX::set_minimum_distance(float min)
 {
 	_min_distance = min;
-}	
+}
 
 void
 MB12XX::set_maximum_distance(float max)
 {
 	_max_distance = max;
-}	
+}
 
 float
 MB12XX::get_minimum_distance()
@@ -284,20 +305,20 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
 
-				/* switching to manual polling */
+			/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
 				stop();
 				_measure_ticks = 0;
 				return OK;
 
-				/* external signalling (DRDY) not supported */
+			/* external signalling (DRDY) not supported */
 			case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
+			/* zero would be bad */
 			case 0:
 				return -EINVAL;
 
-				/* set default/max polling rate */
+			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT: {
 					/* do we need to start internal polling? */
@@ -307,13 +328,14 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
 
-				/* adjust to a legal polling interval in Hz */
+			/* adjust to a legal polling interval in Hz */
 			default: {
 					/* do we need to start internal polling? */
 					bool want_start = (_measure_ticks == 0);
@@ -322,15 +344,17 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
+					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
 						return -EINVAL;
+					}
 
 					/* update interval for next measurement */
 					_measure_ticks = ticks;
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
@@ -338,45 +362,49 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
+		if (_measure_ticks == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 1) || (arg > 100))
-			return -EINVAL;
-		
-		irqstate_t flags = irqsave();
-		if (!_reports->resize(arg)) {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+
 			irqrestore(flags);
-			return -ENOMEM;
+
+			return OK;
 		}
-		irqrestore(flags);
-		
-		return OK;
-	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
-		
+
 	case SENSORIOCRESET:
 		/* XXX implement this */
 		return -EINVAL;
-	
-	case RANGEFINDERIOCSETMINIUMDISTANCE:
-	{
-		set_minimum_distance(*(float *)arg);
-		return 0;
-	}
-	break;
-	case RANGEFINDERIOCSETMAXIUMDISTANCE:
-	{
-		set_maximum_distance(*(float *)arg);
-		return 0;
-	}
-	break;
+
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
+			set_minimum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
+			set_maximum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -391,8 +419,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
@@ -453,14 +482,14 @@ MB12XX::measure()
 	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
 	ret = transfer(&cmd, 1, nullptr, 0);
 
-	if (OK != ret)
-	{
+	if (OK != ret) {
 		perf_count(_comms_errors);
 		log("i2c::transfer returned %d", ret);
 		return ret;
 	}
+
 	ret = OK;
-	
+
 	return ret;
 }
 
@@ -468,34 +497,35 @@ int
 MB12XX::collect()
 {
 	int	ret = -EIO;
-	
+
 	/* read from the sensor */
 	uint8_t val[2] = {0, 0};
-	
+
 	perf_begin(_sample_perf);
-	
+
 	ret = transfer(nullptr, 0, &val[0], 2);
-	
-	if (ret < 0)
-	{
+
+	if (ret < 0) {
 		log("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
 	}
-	
+
 	uint16_t distance = val[0] << 8 | val[1];
-	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
+	float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
 	struct range_finder_report report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
-        report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
 	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
-	
-	/* publish it */
-	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+
+	/* publish it, if we are the primary */
+	if (_range_finder_topic >= 0) {
+		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+	}
 
 	if (_reports->force(&report)) {
 		perf_count(_buffer_overflows);
@@ -519,17 +549,19 @@ MB12XX::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
-	
+
 	/* notify about state change */
 	struct subsystem_info_s info = {
 		true,
 		true,
 		true,
-		SUBSYSTEM_TYPE_RANGEFINDER};
+		SUBSYSTEM_TYPE_RANGEFINDER
+	};
 	static orb_advert_t pub = -1;
 
 	if (pub > 0) {
 		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
 	} else {
 		pub = orb_advertise(ORB_ID(subsystem_info), &info);
 	}
@@ -583,8 +615,9 @@ MB12XX::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
+	if (OK != measure()) {
 		log("measure error");
+	}
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -635,33 +668,37 @@ start()
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver */
 	g_dev = new MB12XX(MB12XX_BUS);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
-	if (OK != g_dev->init())
+	if (OK != g_dev->init()) {
 		goto fail;
+	}
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+	fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		goto fail;
+	}
 
 	exit(0);
 
 fail:
 
-	if (g_dev != nullptr) 
-	{
+	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
@@ -674,15 +711,14 @@ fail:
  */
 void stop()
 {
-	if (g_dev != nullptr)
-	{
+	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
-	}
-	else
-	{
+
+	} else {
 		errx(1, "driver not running");
 	}
+
 	exit(0);
 }
 
@@ -698,24 +734,27 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+	int fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
-		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+	if (fd < 0) {
+		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", MB12XX_DEVICE_PATH);
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
-	if (sz != sizeof(report))
+	if (sz != sizeof(report)) {
 		err(1, "immediate read failed");
+	}
 
 	warnx("single read");
 	warnx("measurement: %0.2f m", (double)report.distance);
 	warnx("time:        %lld", report.timestamp);
 
 	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
@@ -726,20 +765,27 @@ test()
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
 		warnx("measurement: %0.3f", (double)report.distance);
 		warnx("time:        %lld", report.timestamp);
 	}
 
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
 	errx(0, "PASS");
 }
 
@@ -749,16 +795,19 @@ test()
 void
 reset()
 {
-	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+	int fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -769,8 +818,9 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		errx(1, "driver not running");
+	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
@@ -786,32 +836,37 @@ mb12xx_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		mb12xx::start();
-	
-	 /*
-	  * Stop the driver
-	  */
-	 if (!strcmp(argv[1], "stop"))
-		 mb12xx::stop();
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		mb12xx::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		mb12xx::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		mb12xx::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		mb12xx::info();
+	}
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }
diff --git a/src/drivers/mb12xx/module.mk b/src/drivers/mb12xx/module.mk
index 4e00ada02..d751e93e4 100644
--- a/src/drivers/mb12xx/module.mk
+++ b/src/drivers/mb12xx/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= mb12xx
 
 SRCS		= mb12xx.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
index d43e3aef9..5d1f58b85 100644
--- a/src/drivers/md25/md25.cpp
+++ b/src/drivers/md25/md25.cpp
@@ -52,7 +52,7 @@
 #include <arch/board/board.h>
 #include <mavlink/mavlink_log.h>
 
-#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 
@@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu
 	float prev_revolution = md25.getRevolutions1();
 
 	// debug publication
-	control::UOrbPublication<debug_key_value_s> debug_msg(NULL,
+	uORB::Publication<debug_key_value_s> debug_msg(NULL,
 			ORB_ID(debug_key_value));
 
 	// sine wave for motor 1
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
index 1661f67f9..962c6b881 100644
--- a/src/drivers/md25/md25.hpp
+++ b/src/drivers/md25/md25.hpp
@@ -46,7 +46,7 @@
 
 #include <poll.h>
 #include <stdio.h>
-#include <controllib/uorb/UOrbSubscription.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <drivers/device/i2c.h>
 
@@ -270,7 +270,7 @@ private:
 	struct pollfd _controlPoll;
 
 	/** actuator controls subscription */
-	control::UOrbSubscription<actuator_controls_s> _actuators;
+	uORB::Subscription<actuator_controls_s> _actuators;
 
 	// local copy of data from i2c device
 	uint8_t _version;
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 05ae21c1f..07611f903 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -50,6 +50,7 @@
  *    - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
  */
 
+
 #include <nuttx/config.h>
 
 #include <drivers/device/i2c.h>
@@ -78,30 +79,37 @@
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/system_power.h>
 
 #include <drivers/airspeed/airspeed.h>
 
 /* I2C bus address is 1010001x */
 #define I2C_ADDRESS_MS4525DO	0x28	//0x51 /* 7-bit address. */
+#define PATH_MS4525		"/dev/ms4525"
 /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
 #define I2C_ADDRESS_MS5525DSO	0x77	//0x77/* 7-bit address, addr. pin pulled low */
+#define PATH_MS5525		"/dev/ms5525"
 
 /* Register address */
 #define ADDR_READ_MR			0x00	/* write to this address to start conversion */
 
 /* Measurement rate is 100Hz */
+#define MEAS_RATE 100.0f
+#define MEAS_DRIVER_FILTER_FREQ 3.0f
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
 
 class MEASAirspeed : public Airspeed
 {
 public:
-	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
+	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
 
 protected:
 
@@ -113,6 +121,15 @@ protected:
 	virtual int	measure();
 	virtual int	collect();
 
+	math::LowPassFilter2p	_filter;
+
+	/**
+	 * Correct for 5V rail voltage variations
+	 */
+	void voltage_correction(float &diff_pres_pa, float &temperature);
+
+	int _t_system_power;
+	struct system_power_s system_power;
 };
 
 /*
@@ -120,10 +137,12 @@ protected:
  */
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
-MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
-			CONVERSION_INTERVAL)
+MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
+	CONVERSION_INTERVAL, path),
+	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
+	_t_system_power(-1)
 {
-
+	memset(&system_power, 0, sizeof(system_power));
 }
 
 int
@@ -158,23 +177,25 @@ MEASAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
-                perf_count(_comms_errors);
-                perf_end(_sample_perf);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
 		return ret;
 	}
 
-	uint8_t status = val[0] & 0xC0;
+	uint8_t status = (val[0] & 0xC0) >> 6;
 
-	if (status == 2) {
-		log("err: stale data");
-                perf_count(_comms_errors);
-                perf_end(_sample_perf);
-		return ret;
-	} else if (status == 3) {
-		log("err: fault");
-                perf_count(_comms_errors);
-                perf_end(_sample_perf);
-		return ret;                
+	switch (status) {
+	case 0:
+		break;
+
+	case 1:
+		/* fallthrough */
+	case 2:
+		/* fallthrough */
+	case 3:
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return -EAGAIN;
 	}
 
 	int16_t dp_raw = 0, dT_raw = 0;
@@ -183,29 +204,70 @@ MEASAirspeed::collect()
 	dp_raw = 0x3FFF & dp_raw;
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
-	float temperature = ((200 * dT_raw) / 2047) - 50;
+	float temperature = ((200.0f * dT_raw) / 2047) - 50;
 
-	/* calculate differential pressure. As its centered around 8000
-	 * and can go positive or negative, enforce absolute value
-	*/
+	// Calculate differential pressure. As its centered around 8000
+	// and can go positive or negative
 	const float P_min = -1.0f;
 	const float P_max = 1.0f;
-	float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
-        if (diff_press_pa < 0.0f)
-            diff_press_pa = 0.0f;
+	const float PSI_to_Pa = 6894.757f;
+	/*
+	  this equation is an inversion of the equation in the
+	  pressure transfer function figure on page 4 of the datasheet
 
+	  We negate the result so that positive differential pressures
+	  are generated when the bottom port is used as the static
+	  port on the pitot and top port is used as the dynamic port
+	 */
+	float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
+	float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
+
+        // correct for 5V rail voltage if possible
+        voltage_correction(diff_press_pa_raw, temperature);
+
+	// the raw value still should be compensated for the known offset
+	diff_press_pa_raw -= _diff_pres_offset;
+
+	float diff_press_pa = fabsf(diff_press_pa_raw);
+	
+	/*
+	  note that we return both the absolute value with offset
+	  applied and a raw value without the offset applied. This
+	  makes it possible for higher level code to detect if the
+	  user has the tubes connected backwards, and also makes it
+	  possible to correctly use offsets calculated by a higher
+	  level airspeed driver.
+
+	  With the above calculation the MS4525 sensor will produce a
+	  positive number when the top port is used as a dynamic port
+	  and bottom port is used as the static port
+
+	  Also note that the _diff_pres_offset is applied before the
+	  fabsf() not afterwards. It needs to be done this way to
+	  prevent a bias at low speeds, but this also means that when
+	  setting a offset you must set it based on the raw value, not
+	  the offset value
+	 */
+	
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
 	if (diff_press_pa > _max_differential_pressure_pa) {
-	    _max_differential_pressure_pa = diff_press_pa;
+		_max_differential_pressure_pa = diff_press_pa;
 	}
 
 	report.timestamp = hrt_absolute_time();
-        report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
-	report.voltage = 0;
+	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
+
+	/* the dynamics of the filter can make it overshoot into the negative range */
+	if (report.differential_pressure_filtered_pa < 0.0f) {
+		report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
+	}
+
+	report.differential_pressure_raw_pa = diff_press_pa_raw;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
 	if (_airspeed_pub > 0 && !(_pub_blocked)) {
@@ -228,13 +290,17 @@ MEASAirspeed::collect()
 void
 MEASAirspeed::cycle()
 {
+	int ret;
+
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
-		if (OK != collect()) {
+		ret = collect();
+		if (OK != ret) {
 			/* restart the measurement state machine */
 			start();
+			_sensor_ok = false;
 			return;
 		}
 
@@ -258,8 +324,12 @@ MEASAirspeed::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
-		log("measure error");
+	ret = measure();
+	if (OK != ret) {
+		debug("measure error");
+	}
+
+	_sensor_ok = (ret == OK);
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -273,6 +343,62 @@ MEASAirspeed::cycle()
 }
 
 /**
+   correct for 5V rail voltage if the system_power ORB topic is
+   available
+   
+   See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
+   offset versus voltage for 3 sensors
+ */
+void
+MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	if (_t_system_power == -1) {
+		_t_system_power = orb_subscribe(ORB_ID(system_power));
+	}
+	if (_t_system_power == -1) {
+		// not available
+		return;
+	}
+	bool updated = false;
+	orb_check(_t_system_power, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
+	}
+	if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
+		// not valid, skip correction
+		return;
+	}
+
+	const float slope = 65.0f;
+	/*
+	  apply a piecewise linear correction, flattening at 0.5V from 5V
+	 */
+	float voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -0.5f) {
+		voltage_diff = -0.5f;
+	}
+	diff_press_pa -= voltage_diff * slope;
+
+	/*
+	  the temperature masurement varies as well
+	 */
+	const float temp_slope = 0.887f;
+	voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -1.0f) {
+		voltage_diff = -1.0f;
+	}
+	temperature -= voltage_diff * temp_slope;	
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
+}
+
+/**
  * Local functions in support of the shell command.
  */
 namespace meas_airspeed
@@ -300,38 +426,44 @@ start(int i2c_bus)
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver, try the MS4525DO first */
-	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
+	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
 
 	/* check if the MS4525DO was instantiated */
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
 	/* try the MS5525DSO next if init fails */
 	if (OK != g_dev->Airspeed::init()) {
 		delete g_dev;
-		g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO);
+		g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
 
 		/* check if the MS5525DSO was instantiated */
-		if (g_dev == nullptr)
+		if (g_dev == nullptr) {
 			goto fail;
+		}
 
 		/* both versions failed if the init for the MS5525DSO fails, give up */
-		if (OK != g_dev->Airspeed::init())
+		if (OK != g_dev->Airspeed::init()) {
 			goto fail;
+		}
 	}
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		goto fail;
+	}
 
 	exit(0);
 
@@ -342,7 +474,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "no MS4525 airspeed sensor connected");
 }
 
 /**
@@ -376,21 +508,24 @@ test()
 
 	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH);
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
-	if (sz != sizeof(report))
+	if (sz != sizeof(report)) {
 		err(1, "immediate read failed");
+	}
 
 	warnx("single read");
-	warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
@@ -401,23 +536,26 @@ test()
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %d pa", report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}
 
 	/* reset the sensor polling to its default rate */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
 		errx(1, "failed to set default rate");
+	}
 
 	errx(0, "PASS");
 }
@@ -430,14 +568,17 @@ reset()
 {
 	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -448,8 +589,9 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		errx(1, "driver not running");
+	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
@@ -488,32 +630,37 @@ meas_airspeed_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		meas_airspeed::start(i2c_bus);
+	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop"))
+	if (!strcmp(argv[1], "stop")) {
 		meas_airspeed::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		meas_airspeed::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		meas_airspeed::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		meas_airspeed::info();
+	}
 
 	meas_airspeed_usage();
 	exit(0);
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 705e98eea..3996b76a6 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -92,8 +92,20 @@
 #define MOTOR_SPINUP_COUNTER			30
 #define ESC_UORB_PUBLISH_DELAY		500000
 
-
-
+struct MotorData_t {
+	unsigned int Version;                        // the version of the BL (0 = old)
+	unsigned int SetPoint;                       // written by attitude controller
+	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+	float SetPoint_PX4; 			     // Values from PX4
+	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
+	unsigned int ReadMode;                       // select data to read
+	unsigned short RawPwmValue;							// length of PWM pulse
+	// the following bytes must be exactly in that order!
+	unsigned int Current;                        // in 0.1 A steps, read back from BL
+	unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
+	unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
+	unsigned int RoundCount;
+};
 
 class MK : public device::I2C
 {
@@ -119,8 +131,8 @@ public:
 	int		set_motor_count(unsigned count);
 	int		set_motor_test(bool motortest);
 	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
-	int		set_px4mode(int px4mode);
-	int		set_frametype(int frametype);
+	void		set_px4mode(int px4mode);
+	void		set_frametype(int frametype);
 	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
 
 private:
@@ -154,8 +166,10 @@ private:
 
 	actuator_controls_s _controls;
 
+	MotorData_t Motor[MAX_MOTORS];
+
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	static int	control_callback(uintptr_t handle,
 					 uint8_t control_group,
@@ -195,24 +209,6 @@ const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};
 
 int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
 
-struct MotorData_t {
-	unsigned int Version;                        // the version of the BL (0 = old)
-	unsigned int SetPoint;                       // written by attitude controller
-	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
-	float SetPoint_PX4; 			     // Values from PX4
-	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
-	unsigned int ReadMode;                       // select data to read
-	unsigned short RawPwmValue;							// length of PWM pulse
-	// the following bytes must be exactly in that order!
-	unsigned int Current;                        // in 0.1 A steps, read back from BL
-	unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
-	unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
-	unsigned int RoundCount;
-};
-
-MotorData_t Motor[MAX_MOTORS];
-
-
 namespace
 {
 
@@ -226,15 +222,15 @@ MK::MK(int bus, const char *_device_path) :
 	_task(-1),
 	_t_actuators(-1),
 	_t_actuator_armed(-1),
+	_motor(-1),
+	_px4mode(MAPPING_MK),
+	_frametype(FRAME_PLUS),
 	_t_outputs(0),
 	_t_esc_status(0),
 	_num_outputs(0),
+	_primary_pwm_device(false),
 	_motortest(false),
 	_overrideSecurityChecks(false),
-	_motor(-1),
-	_px4mode(MAPPING_MK),
-	_frametype(FRAME_PLUS),
-	_primary_pwm_device(false),
 	_task_should_exit(false),
 	_armed(false),
 	_mixers(nullptr)
@@ -334,13 +330,13 @@ MK::set_update_rate(unsigned rate)
 	return OK;
 }
 
-int
+void
 MK::set_px4mode(int px4mode)
 {
 	_px4mode = px4mode;
 }
 
-int
+void
 MK::set_frametype(int frametype)
 {
 	_frametype = frametype;
@@ -444,9 +440,6 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
 void
 MK::task_main()
 {
-	long update_rate_in_us = 0;
-	float tmpVal = 0;
-
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
 	 * primary PWM output or not.
@@ -487,7 +480,6 @@ MK::task_main()
 		/* handle update rate changes */
 		if (_current_update_rate != _update_rate) {
 			int update_rate_in_ms = int(1000 / _update_rate);
-			update_rate_in_us = long(1000000 / _update_rate);
 
 			/* reject faster than 500 Hz updates */
 			if (update_rate_in_ms < 2) {
@@ -739,7 +731,6 @@ MK::mk_servo_set(unsigned int chan, short val)
 	_retries = 0;
 	uint8_t result[3] = { 0, 0, 0 };
 	uint8_t msg[2] = { 0, 0 };
-	uint8_t rod = 0;
 	uint8_t bytesToSendBL2 = 2;
 
 	tmpVal = val;
@@ -828,7 +819,7 @@ MK::mk_servo_set(unsigned int chan, short val)
 		if (debugCounter == 2000) {
 			debugCounter = 0;
 
-			for (int i = 0; i < _num_outputs; i++) {
+			for (unsigned int i = 0; i < _num_outputs; i++) {
 				if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
 					fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
 				}
@@ -1015,7 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
 		/* copy the current output value from the channel */
-		*(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue;
+		*(servo_position_t *)arg = Motor[cmd - PWM_SERVO_GET(0)].RawPwmValue;
 
 		break;
 
@@ -1173,7 +1164,7 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr
 }
 
 int
-mk_start(unsigned motors, char *device_path)
+mk_start(unsigned motors, const char *device_path)
 {
 	int ret;
 
@@ -1232,7 +1223,7 @@ mkblctrl_main(int argc, char *argv[])
 	bool overrideSecurityChecks = false;
 	bool showHelp = false;
 	bool newMode = false;
-	char *devicepath = "";
+	const char *devicepath = "";
 
 	/*
 	 * optional parameters
diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk
index 3ac263b00..6daa14aa5 100644
--- a/src/drivers/mkblctrl/module.mk
+++ b/src/drivers/mkblctrl/module.mk
@@ -37,6 +37,8 @@
 
 MODULE_COMMAND		= mkblctrl
 
-SRCS				= mkblctrl.cpp
+SRCS			= mkblctrl.cpp
 
 INCLUDE_DIRS		+= $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index ac75682c4..1b3a96a0d 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -55,6 +55,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -71,12 +72,15 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
 
 #define DIR_READ			0x80
 #define DIR_WRITE			0x00
 
 #define MPU_DEVICE_PATH_ACCEL		"/dev/mpu6000_accel"
 #define MPU_DEVICE_PATH_GYRO		"/dev/mpu6000_gyro"
+#define MPU_DEVICE_PATH_ACCEL_EXT	"/dev/mpu6000_accel_ext"
+#define MPU_DEVICE_PATH_GYRO_EXT	"/dev/mpu6000_gyro_ext"
 
 // MPU 6000 registers
 #define MPUREG_WHOAMI			0x75
@@ -177,7 +181,7 @@ class MPU6000_gyro;
 class MPU6000 : public device::SPI
 {
 public:
-	MPU6000(int bus, spi_dev_e device);
+	MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
 	virtual ~MPU6000();
 
 	virtual int		init();
@@ -232,6 +236,8 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	enum Rotation		_rotation;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -345,7 +351,7 @@ private:
 class MPU6000_gyro : public device::CDev
 {
 public:
-	MPU6000_gyro(MPU6000 *parent);
+	MPU6000_gyro(MPU6000 *parent, const char *path);
 	~MPU6000_gyro();
 
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
@@ -368,9 +374,9 @@ private:
 /** driver 'main' command */
 extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
 
-MPU6000::MPU6000(int bus, spi_dev_e device) :
-	SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
-	_gyro(new MPU6000_gyro(this)),
+MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) :
+	SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
+	_gyro(new MPU6000_gyro(this, path_gyro)),
 	_product(0),
 	_call_interval(0),
 	_accel_reports(nullptr),
@@ -391,7 +397,8 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
-	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
+	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_rotation(rotation)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -544,7 +551,7 @@ void MPU6000::reset()
 	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
         irqrestore(state);
 
-	up_udelay(1000);
+	usleep(1000);
 
 	// SAMPLE RATE
 	_set_sample_rate(_sample_rate);
@@ -666,7 +673,9 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 	/* 
 	   choose next highest filter frequency available
 	 */
-	if (frequency_hz <= 5) {
+        if (frequency_hz == 0) {
+		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
+        } else if (frequency_hz <= 5) {
 		filter = BITS_DLPF_CFG_5HZ;
 	} else if (frequency_hz <= 10) {
 		filter = BITS_DLPF_CFG_10HZ;
@@ -922,10 +931,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _accel_filter_x.get_cutoff_freq();
 
 	case ACCELIOCSLOWPASS:
-		
-		// XXX decide on relationship of both filters
-		// i.e. disable the on-chip filter
-		//_set_dlpf_filter((uint16_t)arg);
+		if (arg == 0) {
+			// allow disabling of on-chip filter using
+			// zero as desired filter frequency
+			_set_dlpf_filter(0);
+		}
 		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@@ -1009,8 +1019,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		// XXX check relation to the internal lowpass
-		//_set_dlpf_filter((uint16_t)arg);
+		if (arg == 0) {
+			// allow disabling of on-chip filter using 0
+			// as desired frequency
+			_set_dlpf_filter(0);
+		}
 		return OK;
 
 	case GYROIOCSSCALE:
@@ -1295,6 +1308,9 @@ MPU6000::measure()
 	arb.y = _accel_filter_y.apply(y_in_new);
 	arb.z = _accel_filter_z.apply(z_in_new);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, arb.x, arb.y, arb.z);
+
 	arb.scaling = _accel_range_scale;
 	arb.range_m_s2 = _accel_range_m_s2;
 
@@ -1313,6 +1329,9 @@ MPU6000::measure()
 	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
 	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, grb.x, grb.y, grb.z);
+
 	grb.scaling = _gyro_range_scale;
 	grb.range_rad_s = _gyro_range_rad_s;
 
@@ -1350,8 +1369,8 @@ MPU6000::print_info()
 	_gyro_reports->print_info("gyro queue");
 }
 
-MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
-	CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
+MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
+	CDev("MPU6000_gyro", path),
 	_parent(parent),
 	_gyro_topic(-1),
 	_gyro_class_instance(-1)
@@ -1380,7 +1399,6 @@ MPU6000_gyro::init()
 
 	_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
-out:
 	return ret;
 }
 
@@ -1408,36 +1426,49 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 namespace mpu6000
 {
 
-MPU6000	*g_dev;
+MPU6000	*g_dev_int; // on internal bus
+MPU6000	*g_dev_ext; // on external bus
 
-void	start();
-void	test();
-void	reset();
-void	info();
+void	start(bool, enum Rotation);
+void	test(bool);
+void	reset(bool);
+void	info(bool);
+void	usage();
 
 /**
  * Start the driver.
  */
 void
-start()
+start(bool external_bus, enum Rotation rotation)
 {
 	int fd;
+        MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+	const char *path_gyro  = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
 
-	if (g_dev != nullptr)
+	if (*g_dev_ptr != nullptr)
 		/* if already started, the still command succeeded */
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
+        if (external_bus) {
+#ifdef PX4_SPI_BUS_EXT
+		*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
+#else
+		errx(0, "External SPI not available");
+#endif
+	} else {
+		*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
+	}
 
-	if (g_dev == nullptr)
+	if (*g_dev_ptr == nullptr)
 		goto fail;
 
-	if (OK != g_dev->init())
+	if (OK != (*g_dev_ptr)->init())
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+	fd = open(path_accel, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1450,9 +1481,9 @@ start()
 	exit(0);
 fail:
 
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (*g_dev_ptr != nullptr) {
+            delete (*g_dev_ptr);
+            *g_dev_ptr = nullptr;
 	}
 
 	errx(1, "driver start failed");
@@ -1464,24 +1495,26 @@ fail:
  * and automatic modes.
  */
 void
-test()
+test(bool external_bus)
 {
+	const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+	const char *path_gyro  = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
 	accel_report a_report;
 	gyro_report g_report;
 	ssize_t sz;
 
 	/* get the driver */
-	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+	int fd = open(path_accel, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
-		    MPU_DEVICE_PATH_ACCEL);
+		    path_accel);
 
 	/* get the driver */
-	int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
+	int fd_gyro = open(path_gyro, O_RDONLY);
 
 	if (fd_gyro < 0)
-		err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
+		err(1, "%s open failed", path_gyro);
 
 	/* reset to manual polling */
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -1529,7 +1562,7 @@ test()
 
 	/* XXX add poll-rate tests here too */
 
-	reset();
+	reset(external_bus);
 	errx(0, "PASS");
 }
 
@@ -1537,9 +1570,10 @@ test()
  * Reset the driver.
  */
 void
-reset()
+reset(bool external_bus)
 {
-	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+	const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+	int fd = open(path_accel, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1559,47 +1593,77 @@ reset()
  * Print a little info about the driver.
  */
 void
-info()
+info(bool external_bus)
 {
-	if (g_dev == nullptr)
+        MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
 		errx(1, "driver not running");
 
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
+	printf("state @ %p\n", *g_dev_ptr);
+	(*g_dev_ptr)->print_info();
 
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R rotation");
+}
 
 } // namespace
 
 int
 mpu6000_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+		default:
+			mpu6000::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(argv[1], "start"))
-		mpu6000::start();
+	if (!strcmp(verb, "start"))
+		mpu6000::start(external_bus, rotation);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
-		mpu6000::test();
+	if (!strcmp(verb, "test"))
+		mpu6000::test(external_bus);
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
-		mpu6000::reset();
+	if (!strcmp(verb, "reset"))
+		mpu6000::reset(external_bus);
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
-		mpu6000::info();
+	if (!strcmp(verb, "info"))
+		mpu6000::info(external_bus);
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 0ef056273..fe669b5f5 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -526,6 +527,7 @@ void
 MS5611::cycle()
 {
 	int ret;
+	unsigned dummy;
 
 	/* collection phase? */
 	if (_collect_phase) {
@@ -542,6 +544,8 @@ MS5611::cycle()
 			} else {
 				//log("collection error %d", ret);
 			}
+			/* issue a reset command to the sensor */
+			_interface->ioctl(IOCTL_RESET, dummy);
 			/* reset the collection state machine and try again */
 			start_cycle();
 			return;
@@ -573,6 +577,8 @@ MS5611::cycle()
 	ret = measure();
 	if (ret != OK) {
 		//log("measure error %d", ret);
+		/* issue a reset command to the sensor */
+		_interface->ioctl(IOCTL_RESET, dummy);
 		/* reset the collection state machine and try again */
 		start_cycle();
 		return;
@@ -748,8 +754,8 @@ MS5611::print_info()
 	printf("TEMP:           %d\n", _TEMP);
 	printf("SENS:           %lld\n", _SENS);
 	printf("OFF:            %lld\n", _OFF);
-	printf("P:              %.3f\n", _P);
-	printf("T:              %.3f\n", _T);
+	printf("P:              %.3f\n", (double)_P);
+	printf("T:              %.3f\n", (double)_T);
 	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
 
 	printf("factory_setup             %u\n", _prom.factory_setup);
@@ -770,11 +776,12 @@ namespace ms5611
 
 MS5611	*g_dev;
 
-void	start();
+void	start(bool external_bus);
 void	test();
 void	reset();
 void	info();
 void	calibrate(unsigned altitude);
+void	usage();
 
 /**
  * MS5611 crc4 cribbed from the datasheet
@@ -827,7 +834,7 @@ crc4(uint16_t *n_prom)
  * Start the driver.
  */
 void
-start()
+start(bool external_bus)
 {
 	int fd;
 	prom_u prom_buf;
@@ -840,7 +847,7 @@ start()
 
 	/* create the driver, try SPI first, fall back to I2C if unsuccessful */
 	if (MS5611_spi_interface != nullptr)
-		interface = MS5611_spi_interface(prom_buf);
+            interface = MS5611_spi_interface(prom_buf, external_bus);
 	if (interface == nullptr && (MS5611_i2c_interface != nullptr))
 		interface = MS5611_i2c_interface(prom_buf);
 
@@ -1051,43 +1058,68 @@ calibrate(unsigned altitude)
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+}
+
 } // namespace
 
 int
 ms5611_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "X")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		default:
+			ms5611::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
-		ms5611::start();
+	if (!strcmp(verb, "start"))
+		ms5611::start(external_bus);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(verb, "test"))
 		ms5611::test();
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(verb, "reset"))
 		ms5611::reset();
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(verb, "info"))
 		ms5611::info();
 
 	/*
 	 * Perform MSL pressure calibration given an altitude in metres
 	 */
-	if (!strcmp(argv[1], "calibrate")) {
+	if (!strcmp(verb, "calibrate")) {
 		if (argc < 2)
 			errx(1, "missing altitude");
 
-		long altitude = strtol(argv[2], nullptr, 10);
+		long altitude = strtol(argv[optind+1], nullptr, 10);
 
 		ms5611::calibrate(altitude);
 	}
diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h
index 76fb84de8..f0b3fd61d 100644
--- a/src/drivers/ms5611/ms5611.h
+++ b/src/drivers/ms5611/ms5611.h
@@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom);
 } /* namespace */
 
 /* interface factories */
-extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function;
+extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function;
 extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function;
 
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index 26216e840..5234ce8d6 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -62,7 +62,7 @@
 
 #ifdef PX4_SPIDEV_BARO
 
-device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf);
+device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus);
 
 class MS5611_SPI : public device::SPI
 {
@@ -115,9 +115,17 @@ private:
 };
 
 device::Device *
-MS5611_spi_interface(ms5611::prom_u &prom_buf)
+MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus)
 {
-	return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+	if (external_bus) {
+		#ifdef PX4_SPI_BUS_EXT
+		return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
+		#else
+		return nullptr;
+		#endif
+	} else {
+		return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+	}
 }
 
 MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk
new file mode 100644
index 000000000..825ee9bb7
--- /dev/null
+++ b/src/drivers/pca8574/module.mk
@@ -0,0 +1,6 @@
+#
+# PCA8574 driver for RGB LED
+#
+
+MODULE_COMMAND	 = pca8574
+SRCS		 = pca8574.cpp
diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp
new file mode 100644
index 000000000..904ce18e8
--- /dev/null
+++ b/src/drivers/pca8574/pca8574.cpp
@@ -0,0 +1,554 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pca8574.cpp
+ *
+ * Driver for an 8 I/O controller (PC8574) connected via I2C.
+ *
+ * @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 <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_io_expander.h>
+
+#define PCA8574_ONTIME 120
+#define PCA8574_OFFTIME 120
+#define PCA8574_DEVICE_PATH "/dev/pca8574"
+
+#define ADDR			0x20	///< I2C adress of PCA8574 (default, A0-A2 pulled to GND)
+
+class PCA8574 : public device::I2C
+{
+public:
+	PCA8574(int bus, int pca8574);
+	virtual ~PCA8574();
+
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		info();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+	bool			is_running() { return _running; }
+
+private:
+	work_s			_work;
+
+	uint8_t			_values_out;
+	uint8_t			_values_in;
+	uint8_t			_blinking;
+	uint8_t			_blink_phase;
+
+	enum IOX_MODE		_mode;
+	bool			_running;
+	int			_led_interval;
+	bool			_should_run;
+	bool			_update_out;
+	int			_counter;
+
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	int			send_led_enable(uint8_t arg);
+	int			send_led_values();
+
+	int			get(uint8_t &vals);
+};
+
+/* for now, we only support one PCA8574 */
+namespace
+{
+PCA8574 *g_pca8574;
+}
+
+void pca8574_usage();
+
+extern "C" __EXPORT int pca8574_main(int argc, char *argv[]);
+
+PCA8574::PCA8574(int bus, int pca8574) :
+	I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000),
+	_values_out(0),
+	_values_in(0),
+	_blinking(0),
+	_blink_phase(0),
+	_mode(IOX_MODE_OFF),
+	_running(false),
+	_led_interval(80),
+	_should_run(false),
+	_update_out(false),
+	_counter(0)
+{
+	memset(&_work, 0, sizeof(_work));
+}
+
+PCA8574::~PCA8574()
+{
+}
+
+int
+PCA8574::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	return OK;
+}
+
+int
+PCA8574::probe()
+{
+	uint8_t val;
+	return get(val);
+}
+
+int
+PCA8574::info()
+{
+	int ret = OK;
+
+	return ret;
+}
+
+int
+PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+	case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): {
+			// set the specified on / off state
+			uint8_t position = (1 << (cmd - IOX_SET_VALUE));
+			uint8_t prev = _values_out;
+
+			if (arg) {
+				_values_out |= position;
+
+			} else {
+				_values_out &= ~(position);
+			}
+
+			if (_values_out != prev) {
+				if (_values_out) {
+					_mode = IOX_MODE_ON;
+				}
+				send_led_values();
+			}
+
+			return OK;
+		}
+
+	case IOX_SET_MASK:
+		send_led_enable(arg);
+		return OK;
+
+	case IOX_GET_MASK: {
+			uint8_t val;
+			ret = get(val);
+
+			if (ret == OK) {
+				return val;
+
+			} else {
+				return -1;
+			}
+		}
+
+	case IOX_SET_MODE:
+
+		if (_mode != (IOX_MODE)arg) {
+
+			switch ((IOX_MODE)arg) {
+			case IOX_MODE_OFF:
+				_values_out = 0xFF;
+				break;
+
+			case IOX_MODE_ON:
+				_values_out = 0;
+				break;
+
+			case IOX_MODE_TEST_OUT:
+				break;
+
+			default:
+				return -1;
+			}
+
+			_mode = (IOX_MODE)arg;
+			send_led_values();
+		}
+
+		return OK;
+
+	default:
+		// see if the parent class can make any use of it
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
+	}
+
+	return ret;
+}
+
+
+void
+PCA8574::led_trampoline(void *arg)
+{
+	PCA8574 *rgbl = reinterpret_cast<PCA8574 *>(arg);
+
+	rgbl->led();
+}
+
+/**
+ * Main loop function
+ */
+void
+PCA8574::led()
+{
+	if (_mode == IOX_MODE_TEST_OUT) {
+
+		// we count only seven states
+		_counter &= 0xF;
+		_counter++;
+
+		for (int i = 0; i < 8; i++) {
+			if (i < _counter) {
+				_values_out |= (1 << i);
+
+			} else {
+				_values_out &= ~(1 << i);
+			}
+		}
+
+		_update_out = true;
+		_should_run = true;
+	} else if (_mode == IOX_MODE_OFF) {
+		_update_out = true;
+		_should_run = false;
+	} else {
+
+		// Any of the normal modes
+		if (_blinking > 0) {
+			/* we need to be running to blink */
+			_should_run = true;
+		} else {
+			_should_run = false;
+		}
+	}
+
+	if (_update_out) {
+		uint8_t msg;
+
+		if (_blinking) {
+			msg = (_values_out & _blinking & _blink_phase);
+
+			// wipe out all positions that are marked as blinking
+			msg &= ~(_blinking);
+
+			// fill blink positions
+			msg |= ((_blink_phase) ? _blinking : 0);
+
+			_blink_phase = !_blink_phase;
+		} else {
+			msg = _values_out;
+		}
+
+		int ret = transfer(&msg, sizeof(msg), nullptr, 0);
+
+		if (!ret) {
+			_update_out = false;
+		}
+	}
+
+	// check if any activity remains, else stp
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
+	// re-queue ourselves to run again later
+	_running = true;
+	work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval);
+}
+
+/**
+ * Sent ENABLE flag to LED driver
+ */
+int
+PCA8574::send_led_enable(uint8_t arg)
+{
+
+	int ret = transfer(&arg, sizeof(arg), nullptr, 0);
+
+	return ret;
+}
+
+/**
+ * Send 8 outputs
+ */
+int
+PCA8574::send_led_values()
+{
+	_update_out = true;
+
+	// if not active, kick it
+	if (!_running) {
+		_running = true;
+		work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1);
+	}
+
+	return 0;
+}
+
+int
+PCA8574::get(uint8_t &vals)
+{
+	uint8_t result;
+	int ret;
+
+	ret = transfer(nullptr, 0, &result, 1);
+
+	if (ret == OK) {
+		_values_in = result;
+		vals = result;
+	}
+
+	return ret;
+}
+
+void
+pca8574_usage()
+{
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	warnx("    -a addr (0x%x)", ADDR);
+}
+
+int
+pca8574_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int pca8574adr = ADDR; // 7bit
+
+	int ch;
+
+	// jump over start/off/etc and look at options first
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			pca8574adr = strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			pca8574_usage();
+			exit(0);
+		}
+	}
+
+	if (optind >= argc) {
+		pca8574_usage();
+		exit(1);
+	}
+
+	const char *verb = argv[optind];
+
+	int fd;
+	int ret;
+
+	if (!strcmp(verb, "start")) {
+		if (g_pca8574 != nullptr) {
+			errx(1, "already started");
+		}
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr);
+
+			if (g_pca8574 != nullptr && OK != g_pca8574->init()) {
+				delete g_pca8574;
+				g_pca8574 = nullptr;
+			}
+
+			if (g_pca8574 == nullptr) {
+				// fall back to default bus
+				if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
+					errx(1, "init failed");
+				}
+
+				i2cdevice = PX4_I2C_BUS_LED;
+			}
+		}
+
+		if (g_pca8574 == nullptr) {
+			g_pca8574 = new PCA8574(i2cdevice, pca8574adr);
+
+			if (g_pca8574 == nullptr) {
+				errx(1, "new failed");
+			}
+
+			if (OK != g_pca8574->init()) {
+				delete g_pca8574;
+				g_pca8574 = nullptr;
+				errx(1, "init failed");
+			}
+		}
+
+		exit(0);
+	}
+
+	// need the driver past this point
+	if (g_pca8574 == nullptr) {
+		warnx("not started, run pca8574 start");
+		exit(1);
+	}
+
+	if (!strcmp(verb, "test")) {
+		fd = open(PCA8574_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA8574_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);
+
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "info")) {
+		g_pca8574->info();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "off")) {
+		fd = open(PCA8574_DEVICE_PATH, 0);
+
+		if (fd < 0) {
+			errx(1, "Unable to open " PCA8574_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		fd = open(PCA8574_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA8574_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
+		close(fd);
+
+		// wait until we're not running any more
+		for (unsigned i = 0; i < 15; i++) {
+			if (!g_pca8574->is_running()) {
+				break;
+			}
+
+			usleep(50000);
+			printf(".");
+			fflush(stdout);
+		}
+		printf("\n");
+		fflush(stdout);
+
+		if (!g_pca8574->is_running()) {
+			delete g_pca8574;
+			g_pca8574 = nullptr;
+			exit(0);
+		} else {
+			warnx("stop failed.");
+			exit(1);
+		}
+	}
+
+	if (!strcmp(verb, "val")) {
+		if (argc < 4) {
+			errx(1, "Usage: pca8574 val <channel> <0 or 1>");
+		}
+
+		fd = open(PCA8574_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA8574_DEVICE_PATH);
+		}
+
+		unsigned channel = strtol(argv[2], NULL, 0);
+		unsigned val = strtol(argv[3], NULL, 0);
+
+		if (channel < 8) {
+			ret = ioctl(fd, (IOX_SET_VALUE + channel), val);
+		} else {
+			ret = -1;
+		}
+		close(fd);
+		exit(ret);
+	}
+
+	pca8574_usage();
+	exit(0);
+}
diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/drivers/px4flow/module.mk
index 8d4a40d95..d3062e457 100644
--- a/src/modules/att_pos_estimator_ekf/module.mk
+++ b/src/drivers/px4flow/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   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
@@ -32,11 +32,9 @@
 ############################################################################
 
 #
-# Full attitude / position Extended Kalman Filter
+# Makefile to build the PX4FLOW driver.
 #
 
-MODULE_COMMAND	= att_pos_estimator_ekf
+MODULE_COMMAND	= px4flow
 
-SRCS		= kalman_main.cpp \
-		  KalmanNav.cpp \
-		  params.c
+SRCS		= px4flow.cpp
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
new file mode 100644
index 000000000..f214b5964
--- /dev/null
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -0,0 +1,806 @@
+/****************************************************************************
+ *
+ *   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 px4flow.cpp
+ * @author Dominik Honegger
+ *
+ * Driver for the PX4FLOW module connected via I2C.
+ */
+	 
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_px4flow.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+//#include <uORB/topics/optical_flow.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
+#define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
+//range 0x42 - 0x49
+
+/* PX4FLOW Registers addresses */
+#define PX4FLOW_REG	0x00		/* Measure Register */
+
+#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+//struct i2c_frame
+//{
+//    uint16_t frame_count;
+//    int16_t pixel_flow_x_sum;
+//    int16_t pixel_flow_y_sum;
+//    int16_t flow_comp_m_x;
+//    int16_t flow_comp_m_y;
+//    int16_t qual;
+//    int16_t gyro_x_rate;
+//    int16_t gyro_y_rate;
+//    int16_t gyro_z_rate;
+//    uint8_t gyro_range;
+//    uint8_t sonar_timestamp;
+//    int16_t ground_distance;
+//};
+//
+//struct i2c_frame f;
+
+class PX4FLOW : public device::I2C
+{
+public:
+	PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
+	virtual ~PX4FLOW();
+	
+	virtual int 		init();
+	
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+	
+protected:
+	virtual int			probe();
+
+private:
+
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	
+	orb_advert_t		_px4flow_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+	
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+	
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+	
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+	
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+	
+	
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
+
+PX4FLOW::PX4FLOW(int bus, int address) :
+	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_px4flow_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+	
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+PX4FLOW::~PX4FLOW()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete _reports;
+}
+
+int
+PX4FLOW::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(px4flow_report));
+
+	if (_reports == nullptr)
+		goto out;
+
+	/* get a publish handle on the px4flow topic */
+	struct px4flow_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
+
+	if (_px4flow_topic < 0)
+		debug("failed to create px4flow object. Did you start uOrb?");
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+PX4FLOW::probe()
+{
+	return measure();
+}
+
+int
+PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+		
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _reports->size();
+		
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+	
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct px4flow_report);
+	struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(PX4FLOW_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+PX4FLOW::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = PX4FLOW_REG;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret)
+	{
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		 printf("i2c::transfer flow returned %d");
+		return ret;
+	}
+	ret = OK;
+	
+	return ret;
+}
+
+int
+PX4FLOW::collect()
+{
+	int	ret = -EIO;
+	
+	/* read from the sensor */
+	uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
+	
+	perf_begin(_sample_perf);
+	
+	ret = transfer(nullptr, 0, &val[0], 22);
+	
+	if (ret < 0)
+	{
+		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+	
+//	f.frame_count = val[1] << 8 | val[0];
+//	f.pixel_flow_x_sum= val[3] << 8 | val[2];
+//	f.pixel_flow_y_sum= val[5] << 8 | val[4];
+//	f.flow_comp_m_x= val[7] << 8 | val[6];
+//	f.flow_comp_m_y= val[9] << 8 | val[8];
+//	f.qual= val[11] << 8 | val[10];
+//	f.gyro_x_rate= val[13] << 8 | val[12];
+//	f.gyro_y_rate= val[15] << 8 | val[14];
+//	f.gyro_z_rate= val[17] << 8 | val[16];
+//	f.gyro_range= val[18];
+//	f.sonar_timestamp= val[19];
+//	f.ground_distance= val[21] << 8 | val[20];
+
+	int16_t flowcx = val[7] << 8 | val[6];
+	int16_t flowcy = val[9] << 8 | val[8];
+	int16_t gdist = val[21] << 8 | val[20];
+
+	struct px4flow_report report;
+	report.flow_comp_x_m = float(flowcx)/1000.0f;
+	report.flow_comp_y_m = float(flowcy)/1000.0f;
+	report.flow_raw_x= val[3] << 8 | val[2];
+	report.flow_raw_y= val[5] << 8 | val[4];
+	report.ground_distance_m =float(gdist)/1000.0f;
+	report.quality=  val[10];
+	report.sensor_id = 0;
+	report.timestamp = hrt_absolute_time();
+
+
+	/* publish it */
+	orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
+
+	/* post a report to the ring */
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+PX4FLOW::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
+	
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_OPTICALFLOW};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+PX4FLOW::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+PX4FLOW::cycle_trampoline(void *arg)
+{
+	PX4FLOW *dev = (PX4FLOW *)arg;
+
+	dev->cycle();
+}
+
+void
+PX4FLOW::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&PX4FLOW::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure())
+		log("measure error");
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&PX4FLOW::cycle_trampoline,
+		   this,
+		   USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
+}
+
+void
+PX4FLOW::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace px4flow
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+PX4FLOW	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new PX4FLOW(PX4FLOW_BUS);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) 
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr)
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+	else
+	{
+		errx(1, "driver not running");
+	}
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct px4flow_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+	//	err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
+	warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
+	warnx("time:        %lld", report.timestamp);
+
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
+		warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
+		warnx("time:        %lld", report.timestamp);
+
+
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+px4flow_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		px4flow::start();
+	
+	 /*
+	  * Stop the driver
+	  */
+	 if (!strcmp(argv[1], "stop"))
+		 px4flow::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		px4flow::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		px4flow::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		px4flow::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 0fbd84924..8cc1141aa 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -92,6 +92,7 @@ public:
 		MODE_2PWM,
 		MODE_4PWM,
 		MODE_6PWM,
+		MODE_8PWM,
 	};
 	PX4FMU();
 	virtual ~PX4FMU();
@@ -113,6 +114,9 @@ private:
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 	static const unsigned _max_actuators = 6;
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+	static const unsigned _max_actuators = 8;
+#endif
 
 	Mode		_mode;
 	unsigned	_pwm_default_rate;
@@ -120,19 +124,25 @@ private:
 	uint32_t	_pwm_alt_rate_channels;
 	unsigned	_current_update_rate;
 	int		_task;
-	int		_t_actuators;
-	int		_t_actuator_armed;
-	orb_advert_t	_t_outputs;
+	int		_armed_sub;
+	orb_advert_t	_outputs_pub;
+	actuator_armed_s	_armed;
 	unsigned	_num_outputs;
 	bool		_primary_pwm_device;
 
 	volatile bool	_task_should_exit;
-	bool		_armed;
+	bool		_servo_armed;
 	bool		_pwm_on;
 
 	MixerGroup	*_mixers;
 
-	actuator_controls_s _controls;
+	uint32_t	_groups_required;
+	uint32_t	_groups_subscribed;
+	int		_control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
+	actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
+	orb_id_t	_control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
+	pollfd	_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
+	unsigned	_poll_fds_num;
 
 	pwm_limit_t	_pwm_limit;
 	uint16_t	_failsafe_pwm[_max_actuators];
@@ -143,13 +153,13 @@ private:
 	unsigned	_num_disarmed_set;
 
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	static int	control_callback(uintptr_t handle,
 					 uint8_t control_group,
 					 uint8_t control_index,
 					 float &input);
-
+	void	subscribe();
 	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
 
@@ -197,6 +207,20 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 	{GPIO_VDD_5V_HIPOWER_OC, 0,                       0},
 	{GPIO_VDD_5V_PERIPH_OC,  0,                       0},
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+	/* AeroCore breaks out User GPIOs on J11 */
+	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
+	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
+	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
+	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
+	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
+	{GPIO_GPIO6_INPUT,       GPIO_GPIO6_OUTPUT,       0},
+	{GPIO_GPIO7_INPUT,       GPIO_GPIO7_OUTPUT,       0},
+	{GPIO_GPIO8_INPUT,       GPIO_GPIO8_OUTPUT,       0},
+	{GPIO_GPIO9_INPUT,       GPIO_GPIO9_OUTPUT,       0},
+	{GPIO_GPIO10_INPUT,      GPIO_GPIO10_OUTPUT,      0},
+	{GPIO_GPIO11_INPUT,      GPIO_GPIO11_OUTPUT,      0},
+#endif
 };
 
 const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -216,25 +240,36 @@ PX4FMU::PX4FMU() :
 	_pwm_alt_rate_channels(0),
 	_current_update_rate(0),
 	_task(-1),
-	_t_actuators(-1),
-	_t_actuator_armed(-1),
-	_t_outputs(0),
+	_armed_sub(-1),
+	_outputs_pub(-1),
 	_num_outputs(0),
 	_primary_pwm_device(false),
 	_task_should_exit(false),
-	_armed(false),
+	_servo_armed(false),
 	_pwm_on(false),
 	_mixers(nullptr),
-	_failsafe_pwm({0}),
-	      _disarmed_pwm({0}),
-	      _num_failsafe_set(0),
-	      _num_disarmed_set(0)
+	_groups_required(0),
+	_groups_subscribed(0),
+	_control_subs{-1},
+	_poll_fds_num(0),
+	_failsafe_pwm{0},
+	_disarmed_pwm{0},
+	_num_failsafe_set(0),
+	_num_disarmed_set(0)
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		_min_pwm[i] = PWM_DEFAULT_MIN;
 		_max_pwm[i] = PWM_DEFAULT_MAX;
 	}
 
+	_control_topics[0] = ORB_ID(actuator_controls_0);
+	_control_topics[1] = ORB_ID(actuator_controls_1);
+	_control_topics[2] = ORB_ID(actuator_controls_2);
+	_control_topics[3] = ORB_ID(actuator_controls_3);
+
+	memset(_controls, 0, sizeof(_controls));
+	memset(_poll_fds, 0, sizeof(_poll_fds));
+
 	_debug_enabled = true;
 }
 
@@ -294,7 +329,7 @@ PX4FMU::init()
 	_task = task_spawn_cmd("fmuservo",
 			       SCHED_DEFAULT,
 			       SCHED_PRIORITY_DEFAULT,
-			       2048,
+			       1600,
 			       (main_t)&PX4FMU::task_main_trampoline,
 			       nullptr);
 
@@ -365,6 +400,20 @@ PX4FMU::set_mode(Mode mode)
 
 		break;
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
+		debug("MODE_8PWM");
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0xff);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+		break;
+#endif
+
 	case MODE_NONE:
 		debug("MODE_NONE");
 
@@ -448,32 +497,42 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels)
 }
 
 void
+PX4FMU::subscribe()
+{
+	/* subscribe/unsubscribe to required actuator control groups */
+	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
+	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+	_poll_fds_num = 0;
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (sub_groups & (1 << i)) {
+			warnx("subscribe to actuator_controls_%d", i);
+			_control_subs[i] = orb_subscribe(_control_topics[i]);
+		}
+		if (unsub_groups & (1 << i)) {
+			warnx("unsubscribe from actuator_controls_%d", i);
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+
+		if (_control_subs[i] > 0) {
+			_poll_fds[_poll_fds_num].fd = _control_subs[i];
+			_poll_fds[_poll_fds_num].events = POLLIN;
+			_poll_fds_num++;
+		}
+	}
+}
+
+void
 PX4FMU::task_main()
 {
-	/*
-	 * Subscribe to the appropriate PWM output topic based on whether we are the
-	 * primary PWM output or not.
-	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				     ORB_ID(actuator_controls_1));
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
-	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
-				   &outputs);
-
-	pollfd fds[2];
-	fds[0].fd = _t_actuators;
-	fds[0].events = POLLIN;
-	fds[1].fd = _t_actuator_armed;
-	fds[1].events = POLLIN;
 
 #ifdef HRT_PPM_CHANNEL
 	// rc input, published to ORB
@@ -491,6 +550,12 @@ PX4FMU::task_main()
 
 	/* loop until killed */
 	while (!_task_should_exit) {
+		if (_groups_subscribed != _groups_required) {
+			subscribe();
+			_groups_subscribed = _groups_required;
+			/* force setting update rate */
+			_current_update_rate = 0;
+		}
 
 		/*
 		 * Adjust actuator topic update rate to keep up with
@@ -515,20 +580,23 @@ PX4FMU::task_main()
 			}
 
 			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
-			orb_set_interval(_t_actuators, update_rate_in_ms);
+			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				if (_control_subs[i] > 0) {
+					orb_set_interval(_control_subs[i], update_rate_in_ms);
+				}
+			}
 
 			// set to current max rate, even if we are actually checking slower/faster
 			_current_update_rate = max_rate;
 		}
 
 		/* sleep waiting for data, stopping to check for PPM
-		 * input at 100Hz */
-		int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
+		 * input at 50Hz */
+		int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);
 
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;
 
 		} else if (ret == 0) {
@@ -537,89 +605,100 @@ PX4FMU::task_main()
 
 		} else {
 
-			/* do we have a control update? */
-			if (fds[0].revents & POLLIN) {
+			/* get controls for required topics */
+			unsigned poll_id = 0;
+			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				if (_control_subs[i] > 0) {
+					if (_poll_fds[poll_id].revents & POLLIN) {
+						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
+					}
+					poll_id++;
+				}
+			}
 
-				/* get controls - must always do this to avoid spinning */
-				orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+			/* can we mix? */
+			if (_mixers != nullptr) {
 
-				/* can we mix? */
-				if (_mixers != nullptr) {
+				unsigned num_outputs;
 
-					unsigned num_outputs;
+				switch (_mode) {
+				case MODE_2PWM:
+					num_outputs = 2;
+					break;
 
-					switch (_mode) {
-					case MODE_2PWM:
-						num_outputs = 2;
-						break;
+				case MODE_4PWM:
+					num_outputs = 4;
+					break;
 
-					case MODE_4PWM:
-						num_outputs = 4;
-						break;
+				case MODE_6PWM:
+					num_outputs = 6;
+					break;
 
-					case MODE_6PWM:
-						num_outputs = 6;
-						break;
+				case MODE_8PWM:
+					num_outputs = 8;
+					break;
+				default:
+					num_outputs = 0;
+					break;
+				}
 
-					default:
-						num_outputs = 0;
-						break;
+				/* do mixing */
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.timestamp = hrt_absolute_time();
+
+				/* iterate actuators */
+				for (unsigned i = 0; i < num_outputs; i++) {
+					/* last resort: catch NaN and INF */
+					if ((i >= outputs.noutputs) ||
+						!isfinite(outputs.output[i])) {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = -1.0f;
 					}
+				}
 
-					/* do mixing */
-					outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
-					outputs.timestamp = hrt_absolute_time();
-
-					/* iterate actuators */
-					for (unsigned i = 0; i < num_outputs; i++) {
-						/* last resort: catch NaN, INF and out-of-band errors */
-						if (i >= outputs.noutputs ||
-						    !isfinite(outputs.output[i]) ||
-						    outputs.output[i] < -1.0f ||
-						    outputs.output[i] > 1.0f) {
-							/*
-							 * Value is NaN, INF or out of band - set to the minimum value.
-							 * This will be clearly visible on the servo status and will limit the risk of accidentally
-							 * spinning motors. It would be deadly in flight.
-							 */
-							outputs.output[i] = -1.0f;
-						}
-					}
+				uint16_t pwm_limited[num_outputs];
 
-					uint16_t pwm_limited[num_outputs];
+				/* the PWM limit call takes care of out of band errors and constrains */
+				pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
 
-					pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+				/* output to the servos */
+				for (unsigned i = 0; i < num_outputs; i++) {
+					up_pwm_servo_set(i, pwm_limited[i]);
+				}
 
-					/* output to the servos */
-					for (unsigned i = 0; i < num_outputs; i++) {
-						up_pwm_servo_set(i, pwm_limited[i]);
-					}
+				/* publish mixed control outputs */
+				if (_outputs_pub < 0) {
+					_outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs);
+				} else {
 
-					/* and publish for anyone that cares to see */
-					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs);
 				}
 			}
+		}
 
-			/* how about an arming update? */
-			if (fds[1].revents & POLLIN) {
-				actuator_armed_s aa;
+		/* check arming state */
+		bool updated = false;
+		orb_check(_armed_sub, &updated);
 
-				/* get new value */
-				orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
+		if (updated) {
+			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-				/* update the armed status and check that we're not locked down */
-				bool set_armed = aa.armed && !aa.lockdown;
+			/* update the armed status and check that we're not locked down */
+			bool set_armed = _armed.armed && !_armed.lockdown;
 
-				if (_armed != set_armed)
-					_armed = set_armed;
+			if (_servo_armed != set_armed)
+				_servo_armed = set_armed;
 
-				/* update PWM status if armed or if disarmed PWM values are set */
-				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+			/* update PWM status if armed or if disarmed PWM values are set */
+			bool pwm_on = (_armed.armed || _num_disarmed_set > 0);
 
-				if (_pwm_on != pwm_on) {
-					_pwm_on = pwm_on;
-					up_pwm_servo_arm(pwm_on);
-				}
+			if (_pwm_on != pwm_on) {
+				_pwm_on = pwm_on;
+				up_pwm_servo_arm(pwm_on);
 			}
 		}
 
@@ -661,8 +740,13 @@ PX4FMU::task_main()
 
 	}
 
-	::close(_t_actuators);
-	::close(_t_actuator_armed);
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (_control_subs[i] > 0) {
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+	}
+	::close(_armed_sub);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -684,7 +768,7 @@ PX4FMU::control_callback(uintptr_t handle,
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
-	input = controls->control[control_index];
+	input = controls[control_group].control[control_index];
 	return 0;
 }
 
@@ -707,6 +791,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	case MODE_2PWM:
 	case MODE_4PWM:
 	case MODE_6PWM:
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case MODE_8PWM:
+#endif
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;
 
@@ -736,6 +823,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
+	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 		// these are no-ops, as no safety switch
 		break;
 
@@ -935,6 +1023,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_SET(7):
+	case PWM_SERVO_SET(6):
+		if (_mode < MODE_8PWM) {
+			ret = -EINVAL;
+			break;
+		}
+#endif
+
 	case PWM_SERVO_SET(5):
 	case PWM_SERVO_SET(4):
 		if (_mode < MODE_6PWM) {
@@ -962,6 +1059,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_GET(7):
+	case PWM_SERVO_GET(6):
+		if (_mode < MODE_8PWM) {
+			ret = -EINVAL;
+			break;
+		}
+#endif
+
 	case PWM_SERVO_GET(5):
 	case PWM_SERVO_GET(4):
 		if (_mode < MODE_6PWM) {
@@ -989,12 +1095,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(3):
 	case PWM_SERVO_GET_RATEGROUP(4):
 	case PWM_SERVO_GET_RATEGROUP(5):
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_GET_RATEGROUP(6):
+	case PWM_SERVO_GET_RATEGROUP(7):
+#endif
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
 	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
 		switch (_mode) {
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+		case MODE_8PWM:
+			*(unsigned *)arg = 8;
+			break;
+#endif
+
 		case MODE_6PWM:
 			*(unsigned *)arg = 6;
 			break;
@@ -1040,6 +1156,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			set_mode(MODE_6PWM);
 			break;
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+		case 8:
+			set_mode(MODE_8PWM);
+			break;
+#endif
 
 		default:
 			ret = -EINVAL;
@@ -1052,6 +1173,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		if (_mixers != nullptr) {
 			delete _mixers;
 			_mixers = nullptr;
+			_groups_required = 0;
 		}
 
 		break;
@@ -1060,18 +1182,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
 
 			SimpleMixer *mixer = new SimpleMixer(control_callback,
-							     (uintptr_t)&_controls, mixinfo);
+							     (uintptr_t)_controls, mixinfo);
 
 			if (mixer->check()) {
 				delete mixer;
+				_groups_required = 0;
 				ret = -EINVAL;
 
 			} else {
 				if (_mixers == nullptr)
 					_mixers = new MixerGroup(control_callback,
-								 (uintptr_t)&_controls);
+								 (uintptr_t)_controls);
 
 				_mixers->add_mixer(mixer);
+				_mixers->groups_required(_groups_required);
 			}
 
 			break;
@@ -1082,9 +1206,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			unsigned buflen = strnlen(buf, 1024);
 
 			if (_mixers == nullptr)
-				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
 
 			if (_mixers == nullptr) {
+				_groups_required = 0;
 				ret = -ENOMEM;
 
 			} else {
@@ -1095,7 +1220,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					debug("mixer load failed with %d", ret);
 					delete _mixers;
 					_mixers = nullptr;
+					_groups_required = 0;
 					ret = -EINVAL;
+				} else {
+
+					_mixers->groups_required(_groups_required);
 				}
 			}
 
@@ -1122,10 +1251,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	unsigned count = len / 2;
 	uint16_t values[6];
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	if (count > 8) {
+		// we have at most 8 outputs
+		count = 8;
+	}
+#else
 	if (count > 6) {
 		// we have at most 6 outputs
 		count = 6;
 	}
+#endif
 
 	// allow for misaligned values
 	memcpy(values, buffer, count * 2);
@@ -1400,6 +1536,9 @@ fmu_new_mode(PortMode new_mode)
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 		servo_mode = PX4FMU::MODE_6PWM;
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+		servo_mode = PX4FMU::MODE_8PWM;
+#endif
 		break;
 
 		/* mixed modes supported on v1 board only */
@@ -1645,7 +1784,7 @@ fmu_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(verb, "id")) {
-		char id[12];
+		uint8_t id[12];
 		(void)get_board_serial(id);
 
 		errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
@@ -1714,10 +1853,10 @@ fmu_main(int argc, char *argv[])
 	}
 
 
-	fprintf(stderr, "FMU: unrecognised command, try:\n");
+	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
 	fprintf(stderr, "  mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
 #endif
 	exit(1);
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index 05bc7a5b3..eeb59e1a1 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -4,3 +4,5 @@
 
 MODULE_COMMAND	 = fmu
 SRCS		 = fmu.cpp
+
+MODULE_STACKSIZE = 1200
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 2054faa12..c14f1f783 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -44,3 +44,5 @@ SRCS			= px4io.cpp \
 
 # XXX prune to just get UART registers
 INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MODULE_STACKSIZE = 1200
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 7c7b3dcb7..7d78b0d27 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -72,6 +72,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/param/param.h>
+#include <systemlib/circuit_breaker.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -91,6 +92,8 @@
 
 #include "uploader.h"
 
+#include "modules/dataman/dataman.h"
+
 extern device::Device *PX4IO_i2c_interface() weak_function;
 extern device::Device *PX4IO_serial_interface() weak_function;
 
@@ -195,8 +198,10 @@ public:
 	 * Print IO status.
 	 *
 	 * Print all relevant IO status information
+	 *
+	 * @param extended_status Shows more verbose information (in particular RC config)
 	 */
-	void			print_status();
+	void			print_status(bool extended_status);
 
 	/**
 	 * Fetch and print debug console output.
@@ -527,6 +532,11 @@ PX4IO::~PX4IO()
 	if (_interface != nullptr)
 		delete _interface;
 
+	/* deallocate perfs */
+	perf_free(_perf_update);
+	perf_free(_perf_write);
+	perf_free(_perf_chan_count);
+
 	g_dev = nullptr;
 }
 
@@ -568,9 +578,17 @@ int
 PX4IO::init()
 {
 	int ret;
+	param_t sys_restart_param;
+	int sys_restart_val = DM_INIT_REASON_VOLATILE;
 
 	ASSERT(_task == -1);
 
+	sys_restart_param = param_find("SYS_RESTART_TYPE");
+	if (sys_restart_param != PARAM_INVALID) {
+		/* Indicate restart type is unknown */
+		param_set(sys_restart_param, &sys_restart_val);
+	}
+
 	/* do regular cdev init */
 	ret = CDev::init();
 
@@ -675,6 +693,25 @@ PX4IO::init()
 
 		/* send command to arm system via command API */
 		vehicle_command_s cmd;
+		/* send this to itself */
+		param_t sys_id_param = param_find("MAV_SYS_ID");
+		param_t comp_id_param = param_find("MAV_COMP_ID");
+
+		int32_t sys_id;
+		int32_t comp_id;
+
+		if (param_get(sys_id_param, &sys_id)) {
+			errx(1, "PRM SYSID");
+		}
+
+		if (param_get(comp_id_param, &comp_id)) {
+			errx(1, "PRM CMPID");
+		}
+
+		cmd.target_system = sys_id;
+		cmd.target_component = comp_id;
+		cmd.source_system = sys_id;
+		cmd.source_component = comp_id;
 		/* request arming */
 		cmd.param1 = 1.0f;
 		cmd.param2 = 0;
@@ -684,10 +721,7 @@ PX4IO::init()
 		cmd.param6 = 0;
 		cmd.param7 = 0;
 		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
-		// cmd.target_system = status.system_id;
-		// cmd.target_component = status.component_id;
-		// cmd.source_system = status.system_id;
-		// cmd.source_component = status.component_id;
+
 		/* ask to confirm command */
 		cmd.confirmation =  1;
 
@@ -720,6 +754,11 @@ PX4IO::init()
 			/* keep waiting for state change for 2 s */
 		} while (!safety.armed);
 
+		/* Indicate restart type is in-flight */
+		sys_restart_val = DM_INIT_REASON_IN_FLIGHT;
+		param_set(sys_restart_param, &sys_restart_val);
+
+
 		/* regular boot, no in-air restart, init IO */
 
 	} else {
@@ -745,6 +784,10 @@ PX4IO::init()
 			}
 		}
 
+		/* Indicate restart type is power on */
+		sys_restart_val = DM_INIT_REASON_POWER_ON;
+		param_set(sys_restart_param, &sys_restart_val);
+
 	}
 
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
@@ -756,7 +799,12 @@ PX4IO::init()
 	}
 
 	/* start the IO interface task */
-	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	_task = task_spawn_cmd("px4io",
+					SCHED_DEFAULT,
+					SCHED_PRIORITY_ACTUATOR_OUTPUTS,
+					2000,
+					(main_t)&PX4IO::task_main_trampoline,
+					nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -944,8 +992,40 @@ PX4IO::task_main()
 				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
 
 				if (pret != OK) {
-					log("voltage scaling upload failed");
+					log("vscale upload failed");
+				}
+
+				/* send RC throttle failsafe value to IO */
+				int32_t failsafe_param_val;
+				param_t failsafe_param = param_find("RC_FAILS_THR");
+
+				if (failsafe_param != PARAM_INVALID) {
+
+					param_get(failsafe_param, &failsafe_param_val);
+
+					if (failsafe_param_val > 0) {
+
+						uint16_t failsafe_thr = failsafe_param_val;
+						pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
+						if (pret != OK) {
+							log("failsafe upload failed, FS: %d us", (int)failsafe_thr);
+						}
+					}
+				}
+
+				int32_t safety_param_val;
+				param_t safety_param = param_find("RC_FAILS_THR");
+
+				if (safety_param != PARAM_INVALID) {
+
+					param_get(safety_param, &safety_param_val);
+
+					if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) {
+						/* disable IO safety if circuit breaker asked for it */
+						(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val);
+					}
 				}
+
 			}
 
 		}
@@ -1303,7 +1383,7 @@ void
 PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 {
 	/* only publish if battery has a valid minimum voltage */
-	if (vbatt <= 3300) {
+	if (vbatt <= 4900) {
 		return;
 	}
 
@@ -1332,12 +1412,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 	battery_status.discharged_mah = _battery_mamphour_total;
 	_battery_last_timestamp = battery_status.timestamp;
 
-	/* lazily publish the battery voltage */
-	if (_to_battery > 0) {
-		orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+	/* the announced battery status would conflict with the simulated battery status in HIL */
+	if (!(_pub_blocked)) {
+		/* lazily publish the battery voltage */
+		if (_to_battery > 0) {
+			orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
 
-	} else {
-		_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		} else {
+			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		}
 	}
 }
 
@@ -1397,7 +1480,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	/* we don't have the status bits, so input_source has to be set elsewhere */
 	input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
-	static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+	const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
 	uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
 
 	/*
@@ -1405,8 +1488,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 *
 	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	input_rc.timestamp_publication = hrt_absolute_time();
-
 	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
 
 	if (ret != OK)
@@ -1418,23 +1499,38 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 */
 	channel_count = regs[PX4IO_P_RAW_RC_COUNT];
 
-	if (channel_count != _rc_chan_count)
+	/* limit the channel count */
+	if (channel_count > RC_INPUT_MAX_CHANNELS) {
+		channel_count = RC_INPUT_MAX_CHANNELS;
+	}
+
+	/* count channel count changes to identify signal integrity issues */
+	if (channel_count != _rc_chan_count) {
 		perf_count(_perf_chan_count);
+	}
 
 	_rc_chan_count = channel_count;
 
+	input_rc.timestamp_publication = hrt_absolute_time();
+
 	input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
 	input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
 	input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
 	input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
 	input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
+	input_rc.channel_count = channel_count;
 
 	/* rc_lost has to be set before the call to this function */
-	if (!input_rc.rc_lost && !input_rc.rc_failsafe)
+	if (!input_rc.rc_lost && !input_rc.rc_failsafe) {
 		_rc_last_valid = input_rc.timestamp_publication;
+	}
 
 	input_rc.timestamp_last_signal = _rc_last_valid;
 
+	/* FIELDS NOT SET HERE */
+	/* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */
+
 	if (channel_count > 9) {
 		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
 
@@ -1442,8 +1538,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 			return ret;
 	}
 
-	input_rc.channel_count = channel_count;
-	memcpy(input_rc.values, &regs[prolog], channel_count * 2);
+	/* last thing set are the actual channel values as 16 bit values */
+	for (unsigned i = 0; i < channel_count; i++) {
+		input_rc.values[i] = regs[prolog + i];
+	}
 
 	return ret;
 }
@@ -1476,10 +1574,11 @@ PX4IO::io_publish_raw_rc()
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
-		/* we do not know the RC input, only publish if RC OK flag is set */
-		/* if no raw RC, just don't publish */
-		if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+		/* only keep publishing RC input if we ever got a valid input */
+		if (_rc_last_valid == 0) {
+			/* we have never seen valid RC signals, abort */
 			return OK;
+		}
 	}
 
 	/* lazily advertise on first publication */
@@ -1767,7 +1866,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 }
 
 void
-PX4IO::print_status()
+PX4IO::print_status(bool extended_status)
 {
 	/* basic configuration */
 	printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
@@ -1921,26 +2020,30 @@ PX4IO::print_status()
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
 #endif
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
-	printf("controls");
-
-	for (unsigned i = 0; i < _max_controls; i++)
-		printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
-
-	printf("\n");
-
-	for (unsigned i = 0; i < _max_rc_input; i++) {
-		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
-		uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
-		printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
-		       i,
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
-		       options,
-		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
-		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+	for (unsigned group = 0; group < 4; group++) {
+		printf("controls %u:", group);
+
+		for (unsigned i = 0; i < _max_controls; i++)
+			printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i));
+
+		printf("\n");
+	}
+
+	if (extended_status) {
+		for (unsigned i = 0; i < _max_rc_input; i++) {
+			unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+			uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
+			printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
+			       i,
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
+			       options,
+			       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
+			       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+		}
 	}
 
 	printf("failsafe");
@@ -1957,8 +2060,7 @@ PX4IO::print_status()
 }
 
 int
-PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
-/* Make it obvious that file * isn't used here */
+PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
@@ -2109,6 +2211,10 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 
 	case PWM_SERVO_GET_DISABLE_LOCKDOWN:
 		*(unsigned *)arg = _lockdown_override;
+
+	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+		/* force safety swith off */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
 		break;
 
 	case DSM_BIND_START:
@@ -2370,8 +2476,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	default:
-		/* not a recognized value */
-		ret = -ENOTTY;
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filep, cmd, arg);
+		break;
 	}
 
 	return ret;
@@ -2764,7 +2871,7 @@ monitor(void)
 		if (g_dev != nullptr) {
 
 			printf("\033[2J\033[H"); /* move cursor home and clear screen */
-			(void)g_dev->print_status();
+			(void)g_dev->print_status(false);
 			(void)g_dev->print_debug();
 			printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
 
@@ -3030,7 +3137,7 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 
 		printf("[px4io] loaded\n");
-		g_dev->print_status();
+		g_dev->print_status(true);
 
 		exit(0);
 	}
diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
index 19776c40a..c57ddf65b 100644..100755
--- a/src/drivers/px4io/px4io_i2c.cpp
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -79,7 +79,7 @@ device::Device
 }
 
 PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
-	I2C("PX4IO_i2c", nullptr, bus, address, 320000)
+	I2C("PX4IO_i2c", nullptr, bus, address, 400000)
 {
 	_retries = 3;
 }
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index 43318ca84..c39494fb0 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt()
 		if (_rx_dma_status == _dma_status_waiting) {
 
 			/* verify that the received packet is complete */
-			unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
+			size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
 			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
 				perf_count(_pc_badidle);
 
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index dd8abbac5..d134c0246 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -39,6 +39,7 @@
 #include <nuttx/config.h>
 
 #include <sys/types.h>
+#include <stdlib.h>
 #include <stdint.h>
 #include <stdbool.h>
 #include <assert.h>
@@ -201,9 +202,14 @@ PX4IO_Uploader::upload(const char *filenames[])
 			continue;
 		}
 
-		if (bl_rev <= 2)
+		if (bl_rev <= 2) {
 			ret = verify_rev2(fw_size);
-		else if(bl_rev == 3) {
+		} else if(bl_rev == 3) {
+			ret = verify_rev3(fw_size);
+		} else {
+			/* verify rev 4 and higher still uses the same approach and
+			 * every version *needs* to be verified.
+			 */
 			ret = verify_rev3(fw_size);
 		}
 
@@ -235,9 +241,9 @@ PX4IO_Uploader::upload(const char *filenames[])
 	close(_io_fd);
 	_io_fd = -1;
 
-        // sleep for enough time for the IO chip to boot. This makes
-        // forceupdate more reliably startup IO again after update
-        up_udelay(100*1000);
+	// sleep for enough time for the IO chip to boot. This makes
+	// forceupdate more reliably startup IO again after update
+	up_udelay(100*1000);
 
 	return ret;
 }
@@ -408,11 +414,17 @@ static int read_with_retry(int fd, void *buf, size_t n)
 int
 PX4IO_Uploader::program(size_t fw_size)
 {
-	uint8_t	file_buf[PROG_MULTI_MAX];
+	uint8_t	*file_buf;
 	ssize_t count;
 	int ret;
 	size_t sent = 0;
 
+	file_buf = (uint8_t *)malloc(PROG_MULTI_MAX);
+	if (!file_buf) {
+		log("Can't allocate program buffer");
+		return -ENOMEM;
+	}
+
 	log("programming %u bytes...", (unsigned)fw_size);
 
 	ret = lseek(_fw_fd, 0, SEEK_SET);
@@ -420,8 +432,8 @@ PX4IO_Uploader::program(size_t fw_size)
 	while (sent < fw_size) {
 		/* get more bytes to program */
 		size_t n = fw_size - sent;
-		if (n > sizeof(file_buf)) {
-			n = sizeof(file_buf);
+		if (n > PROG_MULTI_MAX) {
+			n = PROG_MULTI_MAX;
 		}
 		count = read_with_retry(_fw_fd, file_buf, n);
 
@@ -433,8 +445,10 @@ PX4IO_Uploader::program(size_t fw_size)
 			    (int)errno);
 		}
 
-		if (count == 0)
+		if (count == 0) {
+			free(file_buf);
 			return OK;
+		}
 
 		sent += count;
 
@@ -450,9 +464,12 @@ PX4IO_Uploader::program(size_t fw_size)
 
 		ret = get_sync(1000);
 
-		if (ret != OK)
+		if (ret != OK) {
+			free(file_buf);
 			return ret;
+		}
 	}
+	free(file_buf);
 	return OK;
 }
 
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index 4f58891ed..13cbfdfa8 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	default:
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filp, cmd, arg);
 		break;
 	}
 
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index d65a9be36..fdaa7f27b 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -53,7 +53,7 @@
 #include <arch/board/board.h>
 #include <mavlink/mavlink_log.h>
 
-#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 
@@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor)
 		return _motor1Position;
 	} else if (motor == MOTOR_2) {
 		return _motor2Position;
-	}
+	} else {
+		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
+		return NAN;
+    }
 }
 
 float RoboClaw::getMotorSpeed(e_motor motor)
@@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor)
 		return _motor1Speed;
 	} else if (motor == MOTOR_2) {
 		return _motor2Speed;
-	}
+	} else {
+		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
+		return NAN;
+    }
 }
 
 int RoboClaw::setMotorSpeed(e_motor motor, float value)
diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp
index e9f35cf95..58994d6fa 100644
--- a/src/drivers/roboclaw/RoboClaw.hpp
+++ b/src/drivers/roboclaw/RoboClaw.hpp
@@ -45,7 +45,7 @@
 
 #include <poll.h>
 #include <stdio.h>
-#include <controllib/uorb/UOrbSubscription.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <drivers/device/i2c.h>
 
@@ -169,7 +169,7 @@ private:
 	struct pollfd _controlPoll;
 
 	/** actuator controls subscription */
-	control::UOrbSubscription<actuator_controls_s> _actuators;
+	uORB::Subscription<actuator_controls_s> _actuators;
 
 	// private data
 	float _motor1Position;
diff --git a/src/systemcmds/hw_ver/module.mk b/src/drivers/sf0x/module.mk
index 3cc08b6a1..dc2c66d56 100644
--- a/src/systemcmds/hw_ver/module.mk
+++ b/src/drivers/sf0x/module.mk
@@ -32,12 +32,11 @@
 ############################################################################
 
 #
-# Show and test hardware version
+# Makefile to build the Lightware laser range finder driver.
 #
 
-MODULE_COMMAND	 = hw_ver
-SRCS		 = hw_ver.c
+MODULE_COMMAND	= sf0x
 
-MODULE_STACKSIZE = 1024
+SRCS		= sf0x.cpp
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
new file mode 100644
index 000000000..bca1715fa
--- /dev/null
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -0,0 +1,1017 @@
+/****************************************************************************
+ *
+ *   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 sf0x.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Greg Hulands
+ *
+ * Driver for the Lightware SF0x laser rangefinder series
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/device.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+#define SF0X_CONVERSION_INTERVAL	83334
+#define SF0X_TAKE_RANGE_REG		'd'
+#define SF02F_MIN_DISTANCE		0.0f
+#define SF02F_MAX_DISTANCE		40.0f
+#define SF0X_DEFAULT_PORT		"/dev/ttyS2"
+
+class SF0X : public device::CDev
+{
+public:
+	SF0X(const char *port = SF0X_DEFAULT_PORT);
+	virtual ~SF0X();
+
+	virtual int 			init();
+
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int				_measure_ticks;
+	bool				_collect_phase;
+	int				_fd;
+	char				_linebuf[10];
+	unsigned			_linebuf_index;
+	hrt_abstime			_last_read;
+
+	orb_advert_t			_range_finder_topic;
+
+	unsigned			_consecutive_fail_count;
+
+	perf_counter_t			_sample_perf;
+	perf_counter_t			_comms_errors;
+	perf_counter_t			_buffer_overflows;
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE
+	* and SF0X_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int				measure();
+	int				collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void			cycle_trampoline(void *arg);
+
+
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
+
+SF0X::SF0X(const char *port) :
+	CDev("SF0X", RANGE_FINDER_DEVICE_PATH),
+	_min_distance(SF02F_MIN_DISTANCE),
+	_max_distance(SF02F_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_fd(-1),
+	_linebuf_index(0),
+	_last_read(0),
+	_range_finder_topic(-1),
+	_consecutive_fail_count(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
+{
+	/* open fd */
+	_fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+	if (_fd < 0) {
+		warnx("FAIL: laser fd");
+	}
+
+	/* tell it to stop auto-triggering */
+	char stop_auto = ' ';
+	(void)::write(_fd, &stop_auto, 1);
+	usleep(100);
+	(void)::write(_fd, &stop_auto, 1);
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(_fd, &uart_config);
+
+	/* clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+	/* no parity, one stop bit */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	unsigned speed = B9600;
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		warnx("ERR CFG: %d ISPD", termios_state);
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		warnx("ERR CFG: %d OSPD\n", termios_state);
+	}
+
+	if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR baud %d ATTR", termios_state);
+	}
+
+	// disable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+SF0X::~SF0X()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+}
+
+int
+SF0X::init()
+{
+	/* do regular cdev init */
+	if (CDev::init() != OK) {
+		goto out;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
+
+	if (_reports == nullptr) {
+		warnx("mem err");
+		goto out;
+	}
+
+	/* get a publish handle on the range finder topic */
+	struct range_finder_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
+
+	if (_range_finder_topic < 0) {
+		warnx("advert err");
+	}
+
+	/* close the fd */
+	::close(_fd);
+	_fd = -1;
+out:
+	return OK;
+}
+
+int
+SF0X::probe()
+{
+	return measure();
+}
+
+void
+SF0X::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+SF0X::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+SF0X::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+SF0X::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+			/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(SF0X_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(SF0X_CONVERSION_INTERVAL)) {
+						return -EINVAL;
+					}
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0) {
+			return SENSOR_POLLRATE_MANUAL;
+		}
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+
+			irqrestore(flags);
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _reports->size();
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
+			set_minimum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
+			set_maximum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+SF0X::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(SF0X_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+SF0X::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	char cmd = SF0X_TAKE_RANGE_REG;
+	ret = ::write(_fd, &cmd, 1);
+
+	if (ret != sizeof(cmd)) {
+		perf_count(_comms_errors);
+		log("write fail %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+SF0X::collect()
+{
+	int	ret;
+
+	perf_begin(_sample_perf);
+
+	/* clear buffer if last read was too long ago */
+	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
+
+	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
+		_linebuf_index = 0;
+	} else if (_linebuf_index > 0) {
+		/* increment to next read position */
+		_linebuf_index++;
+	}
+
+	/* the buffer for read chars is buflen minus null termination */
+	unsigned readlen = sizeof(_linebuf) - 1;
+
+	/* read from the sensor (uart buffer) */
+	ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
+
+	if (ret < 0) {
+		_linebuf[sizeof(_linebuf) - 1] = '\0';
+		debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+
+		/* only throw an error if we time out */
+		if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
+			return ret;
+
+		} else {
+			return -EAGAIN;
+		}
+	} else if (ret == 0) {
+		return -EAGAIN;
+	}
+
+	/* we did increment the index to the next position already, so just add the additional fields */
+	_linebuf_index += (ret - 1);
+
+	_last_read = hrt_absolute_time();
+
+	if (_linebuf_index < 1) {
+		/* we need at least the two end bytes to make sense of this string */
+		return -EAGAIN;
+
+	} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
+
+		if (_linebuf_index >= readlen - 1) {
+			/* we have a full buffer, but no line ending - abort */
+			_linebuf_index = 0;
+			perf_count(_comms_errors);
+			return -ENOMEM;
+		} else {
+			/* incomplete read, reschedule ourselves */
+			return -EAGAIN;
+		}
+	}
+
+	char *end;
+	float si_units;
+	bool valid;
+
+	/* enforce line ending */
+	unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
+
+	_linebuf[lend] = '\0';
+
+	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
+		si_units = -1.0f;
+		valid = false;
+
+	} else {
+
+		/* we need to find a dot in the string, as we're missing the meters part else */
+		valid = false;
+
+		/* wipe out partially read content from last cycle(s), check for dot */
+		for (unsigned i = 0; i < (lend - 2); i++) {
+			if (_linebuf[i] == '\n') {
+				char buf[sizeof(_linebuf)];
+				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
+				memcpy(_linebuf, buf, (lend + 1) - (i + 1));
+			}
+
+			if (_linebuf[i] == '.') {
+				valid = true;
+			}
+		}
+
+		if (valid) {
+			si_units = strtod(_linebuf, &end);
+
+			/* we require at least 3 characters for a valid number */
+			if (end > _linebuf + 3) {
+				valid = true;
+			} else {
+				si_units = -1.0f;
+				valid = false;
+			}
+		}
+	}
+
+	debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
+
+	/* done with this chunk, resetting - even if invalid */
+	_linebuf_index = 0;
+
+	/* if its invalid, there is no reason to forward the value */
+	if (!valid) {
+		perf_count(_comms_errors);
+		return -EINVAL;
+	}
+
+	struct range_finder_report report;
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	report.timestamp = hrt_absolute_time();
+	report.error_count = perf_event_count(_comms_errors);
+	report.distance = si_units;
+	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
+
+	/* publish it */
+	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+SF0X::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
+
+	// /* notify about state change */
+	// struct subsystem_info_s info = {
+	// 	true,
+	// 	true,
+	// 	true,
+	// 	SUBSYSTEM_TYPE_RANGEFINDER
+	// };
+	// static orb_advert_t pub = -1;
+
+	// if (pub > 0) {
+	// 	orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	// } else {
+	// 	pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	// }
+}
+
+void
+SF0X::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+SF0X::cycle_trampoline(void *arg)
+{
+	SF0X *dev = static_cast<SF0X *>(arg);
+
+	dev->cycle();
+}
+
+void
+SF0X::cycle()
+{
+	/* fds initialized? */
+	if (_fd < 0) {
+		/* open fd */
+		_fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	}
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		int collect_ret = collect();
+
+		if (collect_ret == -EAGAIN) {
+			/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SF0X::cycle_trampoline,
+				   this,
+				   USEC2TICK(1100));
+			return;
+		}
+
+		if (OK != collect_ret) {
+
+			/* we know the sensor needs about four seconds to initialize */
+			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+				log("collection error #%u", _consecutive_fail_count);
+			}
+			_consecutive_fail_count++;
+
+			/* restart the measurement state machine */
+			start();
+			return;
+		} else {
+			/* apparently success */
+			_consecutive_fail_count = 0;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SF0X::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure()) {
+		log("measure error");
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&SF0X::cycle_trampoline,
+		   this,
+		   USEC2TICK(SF0X_CONVERSION_INTERVAL));
+}
+
+void
+SF0X::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %d ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace sf0x
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+SF0X	*g_dev;
+
+void	start(const char *port);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(const char *port)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new SF0X(port);
+
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		goto fail;
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(RANGE_FINDER_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warnx("device open fail");
+		goto fail;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		goto fail;
+	}
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		errx(1, "driver not running");
+	}
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+	}
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		err(1, "immediate read failed");
+	}
+
+	warnx("single read");
+	warnx("val:  %0.2f m", (double)report.distance);
+	warnx("time: %lld", report.timestamp);
+
+	/* start the sensor polling at 2 Hz rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
+		errx(1, "failed to set 2Hz poll rate");
+	}
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		int ret = poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			warnx("timed out");
+			break;
+		}
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
+			break;
+		}
+
+		warnx("read #%u", i);
+		warnx("val:  %0.3f m", (double)report.distance);
+		warnx("time: %lld", report.timestamp);
+	}
+
+	/* reset the sensor polling to the default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "ERR: DEF RATE");
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "failed ");
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		err(1, "driver reset failed");
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "driver poll restart failed");
+	}
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+sf0x_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		if (argc > 2) {
+			sf0x::start(argv[2]);
+
+		} else {
+			sf0x::start(SF0X_DEFAULT_PORT);
+		}
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		sf0x::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		sf0x::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		sf0x::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		sf0x::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 00e46d6b8..aa0dca60c 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -41,6 +41,7 @@
  */
 
 #include <nuttx/config.h>
+#include <board_config.h>
 #include <drivers/device/device.h>
 
 #include <sys/types.h>
@@ -64,6 +65,8 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 
+#include <uORB/topics/system_power.h>
+
 /*
  * Register accessors.
  * For now, no reason not to just use ADC1.
@@ -119,6 +122,8 @@ private:
 	unsigned		_channel_count;
 	adc_msg_s		*_samples;		/**< sample buffer */
 
+	orb_advert_t		_to_system_power;
+
 	/** work trampoline */
 	static void		_tick_trampoline(void *arg);
 
@@ -134,13 +139,16 @@ private:
 	 */
 	uint16_t		_sample(unsigned channel);
 
+	// update system_power ORB topic, only on FMUv2
+	void update_system_power(void);
 };
 
 ADC::ADC(uint32_t channels) :
 	CDev("adc", ADC_DEVICE_PATH),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
+	_sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")),
 	_channel_count(0),
-	_samples(nullptr)
+	_samples(nullptr),
+	_to_system_power(0)
 {
 	_debug_enabled = true;
 
@@ -290,6 +298,43 @@ ADC::_tick()
 	/* scan the channel set and sample each */
 	for (unsigned i = 0; i < _channel_count; i++)
 		_samples[i].am_data = _sample(_samples[i].am_channel);
+	update_system_power();
+}
+
+void
+ADC::update_system_power(void)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	system_power_s system_power;
+	system_power.timestamp = hrt_absolute_time();
+
+	system_power.voltage5V_v = 0;
+	for (unsigned i = 0; i < _channel_count; i++) {
+		if (_samples[i].am_channel == 4) {
+			// it is 2:1 scaled
+			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
+		}
+	}
+
+	// these are not ADC related, but it is convenient to
+	// publish these to the same topic
+	system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
+
+	// note that the valid pins are active low
+	system_power.brick_valid   = !stm32_gpioread(GPIO_VDD_BRICK_VALID);
+	system_power.servo_valid   = !stm32_gpioread(GPIO_VDD_SERVO_VALID);
+
+	// OC pins are active low
+	system_power.periph_5V_OC  = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC);
+	system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC);
+
+	/* lazily publish */
+	if (_to_system_power > 0) {
+		orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
+	} else {
+		_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
+	}
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }
 
 uint16_t
@@ -341,7 +386,7 @@ test(void)
 		err(1, "can't open ADC device");
 
 	for (unsigned i = 0; i < 50; i++) {
-		adc_msg_s data[10];
+		adc_msg_s data[12];
 		ssize_t count = read(fd, data, sizeof(data));
 
 		if (count < 0)
@@ -374,6 +419,10 @@ adc_main(int argc, char *argv[])
 		g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | 
 			(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
 #endif
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+		/* XXX this hardcodes the default channel set for AeroCore - should be configurable */
+		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+#endif
 
 		if (g_adc == nullptr)
 			errx(1, "couldn't allocate the ADC driver");
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index b7c9b89a4..281f918d7 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -94,7 +94,7 @@
 #elif HRT_TIMER == 3
 # define HRT_TIMER_BASE		STM32_TIM3_BASE
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
-# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM3EN
+# define HRT_TIMER_POWER_BIT	RCC_APB1ENR_TIM3EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM3
 # define HRT_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
 # if CONFIG_STM32_TIM3
@@ -141,7 +141,7 @@
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
 # define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM1UP
-# define HRT_TIMER_CLOCK	STM32_APB1_TIM10_CLKIN
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM10_CLKIN
 # if CONFIG_STM32_TIM10
 #  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
 # endif
@@ -150,7 +150,7 @@
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
 # define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
-# define HRT_TIMER_CLOCK	STM32_APB1_TIM11_CLKIN
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM11_CLKIN
 # if CONFIG_STM32_TIM11
 #  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
 # endif
@@ -354,6 +354,9 @@ __EXPORT uint16_t ppm_frame_length = 0;
 __EXPORT unsigned ppm_decoded_channels = 0;
 __EXPORT uint64_t ppm_last_valid_decode = 0;
 
+#define PPM_DEBUG 0
+
+#if PPM_DEBUG
 /* PPM edge history */
 __EXPORT uint16_t ppm_edge_history[32];
 unsigned ppm_edge_next;
@@ -361,6 +364,7 @@ unsigned ppm_edge_next;
 /* PPM pulse history */
 __EXPORT uint16_t ppm_pulse_history[32];
 unsigned ppm_pulse_next;
+#endif
 
 static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
 
@@ -455,10 +459,12 @@ hrt_ppm_decode(uint32_t status)
 	/* how long since the last edge? - this handles counter wrapping implicitely. */
 	width = count - ppm.last_edge;
 
+#if PPM_DEBUG
 	ppm_edge_history[ppm_edge_next++] = width;
 
 	if (ppm_edge_next >= 32)
 		ppm_edge_next = 0;
+#endif
 
 	/*
 	 * if this looks like a start pulse, then push the last set of values
@@ -546,10 +552,12 @@ hrt_ppm_decode(uint32_t status)
 		interval = count - ppm.last_mark;
 		ppm.last_mark = count;
 
+#if PPM_DEBUG
 		ppm_pulse_history[ppm_pulse_next++] = interval;
 
 		if (ppm_pulse_next >= 32)
 			ppm_pulse_next = 0;
+#endif
 
 		/* if the mark-mark timing is out of bounds, abandon the frame */
 		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index f36f2091e..810f4aacc 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -334,6 +334,8 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8";					//battery warning slow
 	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
 	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
+	_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
+	_default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag";			// parachute release
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -344,6 +346,8 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat";	// battery warning slow
 	_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat";	// battery warning fast
 	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
+	_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure";            //fail to arm
+	_tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release";	// parachute release
 }
 
 ToneAlarm::~ToneAlarm()
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index b286e0007..067d77364 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
 	 * Calculate heading error of current position to desired position
 	 */
 
-	/* 
-	 * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution,
-	 * so they need to be scaled by 1e7 and converted to IEEE double precision floating point.
-	 */
-	float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
+	float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon);
 
 	/* calculate heading error */
 	float yaw_err = att->yaw - bearing;
diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk
index d2c48934f..a2a9eb113 100644
--- a/src/examples/fixedwing_control/module.mk
+++ b/src/examples/fixedwing_control/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	= ex_fixedwing_control
 
 SRCS		= main.c \
 		  params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
deleted file mode 100644
index 3125ce246..000000000
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ /dev/null
@@ -1,614 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 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
- * 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 flow_position_control.c
- *
- * Optical flow position controller
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <termios.h>
-#include <time.h>
-#include <math.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/filtered_bottom_flow.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-#include <poll.h>
-#include <mavlink/mavlink_log.h>
-
-#include "flow_position_control_params.h"
-
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-__EXPORT int flow_position_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of position controller.
- */
-static int flow_position_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- * 
- * The actual stack size should be set in the call
- * to task_spawn_cmd().
- */
-int flow_position_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start"))
-	{
-		if (thread_running)
-		{
-			printf("flow position control already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("flow_position_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 6,
-					 4096,
-					 flow_position_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop"))
-	{
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status"))
-	{
-		if (thread_running)
-			printf("\tflow position control app is running\n");
-		else
-			printf("\tflow position control app not started\n");
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-static int
-flow_position_control_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	thread_running = true;
-	static int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[fpc] started");
-
-	uint32_t counter = 0;
-	const float time_scale = powf(10.0f,-6.0f);
-
-	/* structures */
-	struct actuator_armed_s armed;
-	memset(&armed, 0, sizeof(armed));
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct filtered_bottom_flow_s filtered_flow;
-	memset(&filtered_flow, 0, sizeof(filtered_flow));
-	struct vehicle_local_position_s local_pos;
-	memset(&local_pos, 0, sizeof(local_pos));
-	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
-	memset(&speed_sp, 0, sizeof(speed_sp));
-
-	/* subscribe to attitude, motor setpoints and system state */
-	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
-	int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-
-	orb_advert_t speed_sp_pub;
-	bool speed_setpoint_adverted = false;
-
-	/* parameters init*/
-	struct flow_position_control_params params;
-	struct flow_position_control_param_handles param_handles;
-	parameters_init(&param_handles);
-	parameters_update(&param_handles, &params);
-
-	/* init flow sum setpoint */
-	float flow_sp_sumx = 0.0f;
-	float flow_sp_sumy = 0.0f;
-
-	/* init yaw setpoint */
-	float yaw_sp = 0.0f;
-
-	/* init height setpoint */
-	float height_sp = params.height_min;
-
-	/* height controller states */
-	bool start_phase = true;
-	bool landing_initialized = false;
-	float landing_thrust_start = 0.0f;
-
-	/* states */
-	float integrated_h_error = 0.0f;
-	float last_local_pos_z = 0.0f;
-	bool update_flow_sp_sumx = false;
-	bool update_flow_sp_sumy = false;
-	uint64_t last_time = 0.0f;
-	float dt = 0.0f; // s
-
-
-	/* register the perf counter */
-	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime");
-	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval");
-	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
-
-	static bool sensors_ready = false;
-	static bool status_changed = false;
-
-	while (!thread_should_exit)
-	{
-		/* wait for first attitude msg to be sure all data are available */
-		if (sensors_ready)
-		{
-			/* polling */
-			struct pollfd fds[2] = {
-				{ .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator
-				{ .fd = parameter_update_sub,   .events = POLLIN }
-
-			};
-
-			/* wait for a position update, check for exit condition every 500 ms */
-			int ret = poll(fds, 2, 500);
-
-			if (ret < 0)
-			{
-				/* poll error, count it in perf */
-				perf_count(mc_err_perf);
-			}
-			else if (ret == 0)
-			{
-				/* no return value, ignore */
-//				printf("[flow position control] no filtered flow updates\n");
-			}
-			else
-			{
-				/* parameter update available? */
-				if (fds[1].revents & POLLIN)
-				{
-					/* read from param to clear updated flag */
-					struct parameter_update_s update;
-					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
-
-					parameters_update(&param_handles, &params);
-					mavlink_log_info(mavlink_fd,"[fpc] parameters updated.");
-				}
-
-				/* only run controller if position/speed changed */
-				if (fds[0].revents & POLLIN)
-				{
-					perf_begin(mc_loop_perf);
-
-					/* get a local copy of the vehicle state */
-					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-					/* get a local copy of manual setpoint */
-					orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
-					/* get a local copy of attitude */
-					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-					/* get a local copy of filtered bottom flow */
-					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
-					/* get a local copy of local position */
-					orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
-					/* get a local copy of control mode */
-					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-
-					if (control_mode.flag_control_velocity_enabled)
-					{
-						float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
-						float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
-						float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
-
-						if(status_changed == false)
-							mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged");
-
-						status_changed = true;
-
-						/* calc dt */
-						if(last_time == 0)
-						{
-							last_time = hrt_absolute_time();
-							continue;
-						}
-						dt = ((float) (hrt_absolute_time() - last_time)) * time_scale;
-						last_time = hrt_absolute_time();
-
-						/* update flow sum setpoint */
-						if (update_flow_sp_sumx)
-						{
-							flow_sp_sumx = filtered_flow.sumx;
-							update_flow_sp_sumx = false;
-						}
-						if (update_flow_sp_sumy)
-						{
-							flow_sp_sumy = filtered_flow.sumy;
-							update_flow_sp_sumy = false;
-						} 
-
-						/* calc new bodyframe speed setpoints */
-						float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
-						float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d;
-						float speed_limit_height_factor = height_sp; // the settings are for 1 meter
-
-						/* overwrite with rc input if there is any */
-						if(isfinite(manual_pitch) && isfinite(manual_roll))
-						{
-							if(fabsf(manual_pitch) > params.manual_threshold)
-							{
-								speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor;
-								update_flow_sp_sumx = true;
-							}
-
-							if(fabsf(manual_roll) > params.manual_threshold)
-							{
-								speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor;
-								update_flow_sp_sumy = true;
-							}
-						}
-
-						/* limit speed setpoints */
-						if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) &&
-								(speed_body_x >= -params.limit_speed_x * speed_limit_height_factor))
-						{
-							speed_sp.vx = speed_body_x;
-						}
-						else
-						{
-							if(speed_body_x > params.limit_speed_x * speed_limit_height_factor)
-								speed_sp.vx = params.limit_speed_x * speed_limit_height_factor;
-							if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor)
-								speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor;
-						}
-
-						if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) &&
-								(speed_body_y >= -params.limit_speed_y * speed_limit_height_factor))
-						{
-							speed_sp.vy = speed_body_y;
-						}
-						else
-						{
-							if(speed_body_y > params.limit_speed_y * speed_limit_height_factor)
-								speed_sp.vy = params.limit_speed_y * speed_limit_height_factor;
-							if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor)
-								speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor;
-						}
-
-						/* manual yaw change */
-						if(isfinite(manual_yaw) && isfinite(manual.throttle))
-						{
-							if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f)
-							{
-								yaw_sp += manual_yaw * params.limit_yaw_step;
-
-								/* modulo for rotation -pi +pi */
-								if(yaw_sp < -M_PI_F)
-									yaw_sp = yaw_sp + M_TWOPI_F;
-								else if(yaw_sp > M_PI_F)
-									yaw_sp = yaw_sp - M_TWOPI_F;
-							}
-						}
-
-						/* forward yaw setpoint */
-						speed_sp.yaw_sp = yaw_sp;
-
-
-						/* manual height control
-						 * 0-20%: thrust linear down
-						 * 20%-40%: down
-						 * 40%-60%: stabilize altitude
-						 * 60-100%: up
-						 */
-						float thrust_control = 0.0f;
-
-						if (isfinite(manual.throttle))
-						{
-							if (start_phase)
-							{
-								/* control start thrust with stick input */
-								if (manual.throttle < 0.4f)
-								{
-									/* first 40% for up to feedforward */
-									thrust_control = manual.throttle / 0.4f * params.thrust_feedforward;
-								}
-								else
-								{
-									/* second 60% for up to feedforward + 10% */
-									thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward;
-								}
-
-								/* exit start phase if setpoint is reached */
-								if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower)
-								{
-									start_phase = false;
-									/* switch to stabilize */
-									thrust_control = params.thrust_feedforward;
-								}
-							}
-							else
-							{
-								if (manual.throttle < 0.2f)
-								{
-									/* landing initialization */
-									if (!landing_initialized)
-									{
-										/* consider last thrust control to avoid steps */
-										landing_thrust_start = speed_sp.thrust_sp;
-										landing_initialized = true;
-									}
-
-									/* set current height as setpoint to avoid steps */
-									if (-local_pos.z > params.height_min)
-										height_sp = -local_pos.z;
-									else
-										height_sp = params.height_min;
-
-									/* lower 20% stick range controls thrust down */
-									thrust_control = manual.throttle / 0.2f * landing_thrust_start;
-
-									/* assume ground position here */
-									if (thrust_control < 0.1f)
-									{
-										/* reset integral if on ground */
-										integrated_h_error = 0.0f;
-										/* switch to start phase */
-										start_phase = true;
-										/* reset height setpoint */
-										height_sp = params.height_min;
-									}
-								}
-								else
-								{
-									/* stabilized mode */
-									landing_initialized = false;
-
-									/* calc new thrust with PID */
-									float height_error = (local_pos.z - (-height_sp));
-
-									/* update height setpoint if needed*/
-									if (manual.throttle < 0.4f)
-									{
-										/* down */
-										if (height_sp > params.height_min + params.height_rate &&
-												fabsf(height_error) < params.limit_height_error)
-											height_sp -= params.height_rate * dt;
-									}
-
-									if (manual.throttle > 0.6f)
-									{
-										/* up */
-										if (height_sp < params.height_max &&
-												fabsf(height_error) < params.limit_height_error)
-											height_sp += params.height_rate * dt;
-									}
-
-									/* instead of speed limitation, limit height error (downwards) */
-									if(height_error > params.limit_height_error)
-										height_error = params.limit_height_error;
-									else if(height_error < -params.limit_height_error)
-										height_error = -params.limit_height_error;
-
-									integrated_h_error = integrated_h_error + height_error;
-									float integrated_thrust_addition = integrated_h_error * params.height_i;
-
-									if(integrated_thrust_addition > params.limit_thrust_int)
-										integrated_thrust_addition = params.limit_thrust_int;
-									if(integrated_thrust_addition < -params.limit_thrust_int)
-										integrated_thrust_addition = -params.limit_thrust_int;
-
-									float height_speed = last_local_pos_z - local_pos.z;
-									float thrust_diff = height_error * params.height_p - height_speed * params.height_d;
-
-									thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition;
-
-									/* add attitude component
-									 * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM
-									 */
-//									// TODO problem with attitude
-//									if (att.R_valid && att.R[2][2] > 0)
-//										thrust_control = thrust_control / att.R[2][2];
-
-									/* set thrust lower limit */
-									if(thrust_control < params.limit_thrust_lower)
-										thrust_control = params.limit_thrust_lower;
-								}
-							}
-
-							/* set thrust upper limit */
-							if(thrust_control > params.limit_thrust_upper)
-								thrust_control = params.limit_thrust_upper;
-						}
-						/* store actual height for speed estimation */
-						last_local_pos_z = local_pos.z;
-
-						speed_sp.thrust_sp =  thrust_control; //manual.throttle;
-						speed_sp.timestamp = hrt_absolute_time();
-
-						/* publish new speed setpoint */
-						if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp))
-						{
-
-							if(speed_setpoint_adverted)
-							{
-								orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp);
-							}
-							else
-							{
-								speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp);
-								speed_setpoint_adverted = true;
-							}
-						}
-						else
-						{
-							warnx("NaN in flow position controller!");
-						}
-					}
-					else
-					{
-						/* in manual or stabilized state just reset speed and flow sum setpoint */
-						//mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy);
-						if(status_changed == true)
-							mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged.");
-
-						status_changed = false;
-						speed_sp.vx = 0.0f;
-						speed_sp.vy = 0.0f;
-						flow_sp_sumx = filtered_flow.sumx;
-						flow_sp_sumy = filtered_flow.sumy;
-						if(isfinite(att.yaw))
-						{
-							yaw_sp = att.yaw;
-							speed_sp.yaw_sp = att.yaw;
-						}
-						if(isfinite(manual.throttle))
-							speed_sp.thrust_sp = manual.throttle;
-					}
-					/* measure in what intervals the controller runs */
-					perf_count(mc_interval_perf);
-					perf_end(mc_loop_perf);
-				}
-			}
-
-			counter++;
-		}
-		else
-		{
-			/* sensors not ready waiting for first attitude msg */
-
-			/* polling */
-			struct pollfd fds[1] = {
-				{ .fd = vehicle_attitude_sub, .events = POLLIN },
-			};
-
-			/* wait for a flow msg, check for exit condition every 5 s */
-			int ret = poll(fds, 1, 5000);
-
-			if (ret < 0)
-			{
-				/* poll error, count it in perf */
-				perf_count(mc_err_perf);
-			}
-			else if (ret == 0)
-			{
-				/* no return value, ignore */
-				mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n");
-			}
-			else
-			{
-				if (fds[0].revents & POLLIN)
-				{
-					sensors_ready = true;
-					mavlink_log_info(mavlink_fd,"[fpc] initialized.\n");
-				}
-			}
-		}
-	}
-
-	mavlink_log_info(mavlink_fd,"[fpc] ending now...\n");
-
-	thread_running = false;
-
-	close(parameter_update_sub);
-	close(vehicle_attitude_sub);
-	close(vehicle_local_position_sub);
-	close(armed_sub);
-	close(control_mode_sub);
-	close(manual_control_setpoint_sub);
-	close(speed_sp_pub);
-
-	perf_print_counter(mc_loop_perf);
-	perf_free(mc_loop_perf);
-
-	fflush(stdout);
-	return 0;
-}
-
diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c
deleted file mode 100644
index eb1473647..000000000
--- a/src/examples/flow_position_control/flow_position_control_params.c
+++ /dev/null
@@ -1,124 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 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
- * 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 flow_position_control_params.c
- */
-
-#include "flow_position_control_params.h"
-
-/* controller parameters */
-
-// Position control P gain
-PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f);
-// Position control D / damping gain
-PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f);
-// Altitude control P gain
-PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f);
-// Altitude control I (integrator) gain
-PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f);
-// Altitude control D gain
-PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f);
-// Altitude control rate limiter
-PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f);
-// Altitude control minimum altitude
-PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f);
-// Altitude control maximum altitude (higher than 1.5m is untested)
-PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f);
-// Altitude control feed forward throttle - adjust to the
-// throttle position (0..1) where the copter hovers in manual flight
-PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight
-PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f);
-PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f);
-PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f);
-PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f);
-PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f);
-PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f);
-PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f);
-PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f);
-
-
-int parameters_init(struct flow_position_control_param_handles *h)
-{
-	/* PID parameters */
-	h->pos_p	 			=	param_find("FPC_POS_P");
-	h->pos_d 				=	param_find("FPC_POS_D");
-	h->height_p 			=	param_find("FPC_H_P");
-	h->height_i 			=	param_find("FPC_H_I");
-	h->height_d 			=	param_find("FPC_H_D");
-	h->height_rate 			=	param_find("FPC_H_RATE");
-	h->height_min			=	param_find("FPC_H_MIN");
-	h->height_max			=	param_find("FPC_H_MAX");
-	h->thrust_feedforward 	=	param_find("FPC_T_FFWD");
-	h->limit_speed_x 		=	param_find("FPC_L_S_X");
-	h->limit_speed_y 		=	param_find("FPC_L_S_Y");
-	h->limit_height_error	=	param_find("FPC_L_H_ERR");
-	h->limit_thrust_int 	=	param_find("FPC_L_TH_I");
-	h->limit_thrust_upper 	=	param_find("FPC_L_TH_U");
-	h->limit_thrust_lower 	=	param_find("FPC_L_TH_L");
-	h->limit_yaw_step		=	param_find("FPC_L_YAW_STEP");
-	h->manual_threshold 	=	param_find("FPC_MAN_THR");
-	h->rc_scale_pitch		=   param_find("RC_SCALE_PITCH");
-	h->rc_scale_roll		=   param_find("RC_SCALE_ROLL");
-	h->rc_scale_yaw			=   param_find("RC_SCALE_YAW");
-
-	return OK;
-}
-
-int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p)
-{
-	param_get(h->pos_p, &(p->pos_p));
-	param_get(h->pos_d, &(p->pos_d));
-	param_get(h->height_p, &(p->height_p));
-	param_get(h->height_i, &(p->height_i));
-	param_get(h->height_d, &(p->height_d));
-	param_get(h->height_rate, &(p->height_rate));
-	param_get(h->height_min, &(p->height_min));
-	param_get(h->height_max, &(p->height_max));
-	param_get(h->thrust_feedforward, &(p->thrust_feedforward));
-	param_get(h->limit_speed_x, &(p->limit_speed_x));
-	param_get(h->limit_speed_y, &(p->limit_speed_y));
-	param_get(h->limit_height_error, &(p->limit_height_error));
-	param_get(h->limit_thrust_int, &(p->limit_thrust_int));
-	param_get(h->limit_thrust_upper, &(p->limit_thrust_upper));
-	param_get(h->limit_thrust_lower, &(p->limit_thrust_lower));
-	param_get(h->limit_yaw_step, &(p->limit_yaw_step));
-	param_get(h->manual_threshold, &(p->manual_threshold));
-	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
-	param_get(h->rc_scale_roll, &(p->rc_scale_roll));
-	param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
-
-	return OK;
-}
diff --git a/src/examples/flow_position_control/flow_position_control_params.h b/src/examples/flow_position_control/flow_position_control_params.h
deleted file mode 100644
index d0c8fc722..000000000
--- a/src/examples/flow_position_control/flow_position_control_params.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 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
- * 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 flow_position_control_params.h
- * 
- * Parameters for position controller
- */
-
-#include <systemlib/param/param.h>
-
-struct flow_position_control_params {
-	float pos_p;
-	float pos_d;
-	float height_p;
-	float height_i;
-	float height_d;
-	float height_rate;
-	float height_min;
-	float height_max;
-	float thrust_feedforward;
-	float limit_speed_x;
-	float limit_speed_y;
-	float limit_height_error;
-	float limit_thrust_int;
-	float limit_thrust_upper;
-	float limit_thrust_lower;
-	float limit_yaw_step;
-	float manual_threshold;
-	float rc_scale_pitch;
-	float rc_scale_roll;
-	float rc_scale_yaw;
-};
-
-struct flow_position_control_param_handles {
-	param_t pos_p;
-	param_t pos_d;
-	param_t height_p;
-	param_t height_i;
-	param_t height_d;
-	param_t height_rate;
-	param_t height_min;
-	param_t height_max;
-	param_t thrust_feedforward;
-	param_t limit_speed_x;
-	param_t limit_speed_y;
-	param_t limit_height_error;
-	param_t limit_thrust_int;
-	param_t limit_thrust_upper;
-	param_t limit_thrust_lower;
-	param_t limit_yaw_step;
-	param_t manual_threshold;
-	param_t rc_scale_pitch;
-	param_t rc_scale_roll;
-	param_t rc_scale_yaw;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct flow_position_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p);
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index 495c415f2..c775428ef 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -65,6 +65,7 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/filtered_bottom_flow.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
 #include <poll.h>
 
 #include "flow_position_estimator_params.h"
@@ -109,9 +110,9 @@ int flow_position_estimator_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		daemon_task = task_spawn_cmd("flow_position_estimator",
-					 SCHED_RR,
+					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 5,
-					 4096,
+					 4000,
 					 flow_position_estimator_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/examples/px4_daemon_app/module.mk b/src/examples/px4_daemon_app/module.mk
index 5f8aa73d5..fc4223142 100644
--- a/src/examples/px4_daemon_app/module.mk
+++ b/src/examples/px4_daemon_app/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND		= px4_daemon_app
 
 SRCS			= px4_daemon_app.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c
index 53f1b4a9a..3eaf14148 100644
--- a/src/examples/px4_daemon_app/px4_daemon_app.c
+++ b/src/examples/px4_daemon_app/px4_daemon_app.c
@@ -98,7 +98,7 @@ int px4_daemon_app_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("daemon",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT,
-					 4096,
+					 2000,
 					 px4_daemon_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/examples/px4_mavlink_debug/module.mk b/src/examples/px4_mavlink_debug/module.mk
index fefd61496..c7ef97fc4 100644
--- a/src/examples/px4_mavlink_debug/module.mk
+++ b/src/examples/px4_mavlink_debug/module.mk
@@ -37,4 +37,6 @@
 
 MODULE_COMMAND		= px4_mavlink_debug
 
-SRCS			= px4_mavlink_debug.c
\ No newline at end of file
+SRCS			= px4_mavlink_debug.c
+
+MODULE_STACKSIZE = 2000
diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c
index 44e6dc7f3..4e9f099ed 100644
--- a/src/examples/px4_simple_app/px4_simple_app.c
+++ b/src/examples/px4_simple_app/px4_simple_app.c
@@ -41,6 +41,7 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <poll.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
diff --git a/src/modules/controllib/block/List.hpp b/src/include/containers/List.hpp
index 96b0b94d1..13cbda938 100644
--- a/src/modules/controllib/block/List.hpp
+++ b/src/include/containers/List.hpp
@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Node.h
+ * @file List.hpp
  *
- * A node of a linked list.
+ * A linked list.
  */
 
 #pragma once
@@ -43,7 +43,7 @@ template<class T>
 class __EXPORT ListNode
 {
 public:
-	ListNode() : _sibling(NULL) {
+	ListNode() : _sibling(nullptr) {
 	}
 	void setSibling(T sibling) { _sibling = sibling; }
 	T getSibling() { return _sibling; }
diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h
index 5054937e0..6d56c546a 100644
--- a/src/include/mavlink/mavlink_log.h
+++ b/src/include/mavlink/mavlink_log.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.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
@@ -100,6 +99,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  */
 #define mavlink_log_info(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
 
+
 struct mavlink_logmessage {
 	char text[MAVLINK_LOG_MAXLEN + 1];
 	unsigned char severity;
@@ -112,6 +112,7 @@ struct mavlink_logbuffer {
 	struct mavlink_logmessage *elems;
 };
 
+__BEGIN_DECLS
 void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
 
 void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb);
@@ -125,6 +126,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
 
 void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
+__END_DECLS
 
 #endif
 
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
index b078562c2..e17715733 100644
--- a/src/lib/conversion/rotation.cpp
+++ b/src/lib/conversion/rotation.cpp
@@ -41,22 +41,153 @@
 #include "rotation.h"
 
 __EXPORT void
-get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix)
 {
-	/* first set to zero */
-	rot_matrix->Matrix::zero(3, 3);
-
 	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
 	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
 	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
 
-	math::EulerAngles euler(roll, pitch, yaw);
+	rot_matrix->from_euler(roll, pitch, yaw);
+}
 
-	math::Dcm R(euler);
+#define HALF_SQRT_2 0.70710678118654757f
 
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < 3; j++) {
-			(*rot_matrix)(i, j) = R(i, j);
-		}
-	}
+__EXPORT void
+rotate_3f(enum Rotation rot, float &x, float &y, float &z)
+{
+    float tmp;
+    switch (rot) {
+    case ROTATION_NONE:
+    case ROTATION_MAX:
+        return;
+    case ROTATION_YAW_45: {
+        tmp = HALF_SQRT_2*(x - y);
+        y   = HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_YAW_90: {
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_YAW_135: {
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(x - y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_YAW_180:
+        x = -x; y = -y;
+        return;
+    case ROTATION_YAW_225: {
+        tmp = HALF_SQRT_2*(y - x);
+        y   = -HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_YAW_270: {
+        tmp = x; x = y; y = -tmp;
+        return;
+    }
+    case ROTATION_YAW_315: {
+        tmp = HALF_SQRT_2*(x + y);
+        y   = HALF_SQRT_2*(y - x);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_180: {
+        y = -y; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_45: {
+        tmp = HALF_SQRT_2*(x + y);
+        y   = HALF_SQRT_2*(x - y);
+        x = tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_90: {
+        tmp = x; x = y; y = tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_135: {
+        tmp = HALF_SQRT_2*(y - x);
+        y   = HALF_SQRT_2*(y + x);
+        x = tmp; z = -z;
+        return;
+    }
+    case ROTATION_PITCH_180: {
+        x = -x; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_225: {
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(y - x);
+        x = tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_270: {
+        tmp = x; x = -y; y = -tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_315: {
+        tmp =  HALF_SQRT_2*(x - y);
+        y   = -HALF_SQRT_2*(x + y);
+        x = tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_90: {
+        tmp = z; z = y; y = -tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_45: {
+        tmp = z; z = y; y = -tmp;
+        tmp = HALF_SQRT_2*(x - y);
+        y   = HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_90: {
+        tmp = z; z = y; y = -tmp;
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_135: {
+        tmp = z; z = y; y = -tmp;
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(x - y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270: {
+        tmp = z; z = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_45: {
+        tmp = z; z = -y; y = tmp;
+        tmp = HALF_SQRT_2*(x - y);
+        y   = HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_90: {
+        tmp = z; z = -y; y = tmp;
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_135: {
+        tmp = z; z = -y; y = tmp;
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(x - y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_PITCH_90: {
+        tmp = z; z = -x; x = tmp;
+        return;
+    }
+    case ROTATION_PITCH_270: {
+        tmp = z; z = x; x = -tmp;
+        return;
+    }
+    }
 }
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 85c63c0fc..5187b448f 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -116,6 +116,14 @@ const rot_lookup_t rot_lookup[] = {
  * Get the rotation matrix
  */
 __EXPORT void
-get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix);
+
+
+/**
+ * rotate a 3 element float vector in-place
+ */
+__EXPORT void
+rotate_3f(enum Rotation rot, float &x, float &y, float &z);
+
 
 #endif /* ROTATION_H_ */
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 2eb58abd6..46db788a6 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -45,38 +45,39 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_PitchController::ECL_PitchController() :
 	_last_run(0),
+	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate_pos(0.0f),
+	_max_rate_neg(0.0f),
+	_roll_ff(0.0f),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_max_deflection_rad(math::radians(45.0f))
+	_bodyrate_setpoint(0.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
 {
 }
 
-float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
-				   bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+ECL_PitchController::~ECL_PitchController()
 {
-	/* get the usual dt estimate */
-	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
-	_last_run = ecl_absolute_time();
-	float dt = (float)dt_micros * 1e-6f;
-
-	/* lock integral for long intervals */
-	if (dt_micros > 500000)
-		lock_integrator = true;
-
-	float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_i_rate = _k_i * _tc;
+	perf_free(_nonfinite_input_perf);
+}
 
-	/* input conditioning */
-	if (!isfinite(airspeed)) {
-		/* airspeed is NaN, +- INF or not available, pick center of band */
-		airspeed = 0.5f * (airspeed_min + airspeed_max);
-	} else if (airspeed < airspeed_min) {
-		airspeed = airspeed_min;
+float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
+		perf_count(_nonfinite_input_perf);
+		warnx("not controlling pitch");
+		return _rate_setpoint;
 	}
 
 	/* flying inverted (wings upside down) ? */
@@ -100,34 +101,83 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	}
 
 	/* calculate the offset in the rate resulting from rolling  */
+	//xxx needs explanation and conversion to body angular rates or should be removed
 	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
 				tanf(roll) * sinf(roll)) * _roll_ff;
 	if (inverted)
 		turn_offset = -turn_offset;
 
+	/* Calculate the error */
 	float pitch_error = pitch_setpoint - pitch;
-	/* rate setpoint from current error and time constant */
-	_rate_setpoint = pitch_error / _tc;
+
+	/*  Apply P controller: rate setpoint from current error and time constant */
+	_rate_setpoint =  pitch_error / _tc;
 
 	/* add turn offset */
 	_rate_setpoint += turn_offset;
 
-	_rate_error = _rate_setpoint - pitch_rate;
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
+		if (_rate_setpoint > 0.0f) {
+			_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
+		} else {
+			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
+		}
 
-	float ilimit_scaled = _integrator_max * scaler;
+	}
 
-	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+	return _rate_setpoint;
+}
 
-		float id = _rate_error * k_i_rate * dt * scaler;
+float ECL_PitchController::control_bodyrate(float roll, float pitch,
+		float pitch_rate, float yaw_rate,
+		float yaw_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
+				isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
+				isfinite(airspeed_max) && isfinite(scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
+
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
+
+	/* Transform estimation to body angular rates */
+	float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
+
+	_rate_error = _bodyrate_setpoint - pitch_bodyrate;
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase into the
-		 * wrong direction if actuator is at limit
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
 		 */
-		if (_last_output < -_max_deflection_rad) {
+		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
-		} else if (_last_output > _max_deflection_rad) {
+		} else if (_last_output > 1.0f) {
 			/* only allow motion to center: decrease value */
 			id = math::min(id, 0.0f);
 		}
@@ -136,11 +186,14 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
-	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
-
-	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
+//	warnx("roll: _last_output %.4f", (double)_last_output);
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_PitchController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 1e6cec6a1..39b9f9d03 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -36,6 +36,7 @@
  * Definition of a simple orthogonal pitch PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Acknowledgements:
  *
@@ -50,14 +51,22 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
-class __EXPORT ECL_PitchController
+class __EXPORT ECL_PitchController //XXX: create controller superclass
 {
 public:
 	ECL_PitchController();
 
-	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
-		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+	~ECL_PitchController();
+
+	float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
+
+
+	float control_bodyrate(float roll, float pitch,
+			float pitch_rate, float yaw_rate,
+			float yaw_rate_setpoint,
+			float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
 
 	void reset_integrator();
 
@@ -67,21 +76,27 @@ public:
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate_pos(float max_rate_pos) {
 		_max_rate_pos = max_rate_pos;
 	}
+
 	void set_max_rate_neg(float max_rate_neg) {
 		_max_rate_neg = max_rate_neg;
 	}
+
 	void set_roll_ff(float roll_ff) {
 		_roll_ff = roll_ff;
 	}
@@ -94,13 +109,17 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 
 	uint64_t _last_run;
 	float _tc;
 	float _k_p;
 	float _k_i;
-	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate_pos;
 	float _max_rate_neg;
@@ -109,7 +128,8 @@ private:
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
+	perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 0b1ffa7a4..9894a34d7 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -45,22 +45,66 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_RollController::ECL_RollController() :
 	_last_run(0),
 	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_max_deflection_rad(math::radians(45.0f))
+	_bodyrate_setpoint(0.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
 {
+}
 
+ECL_RollController::~ECL_RollController()
+{
+	perf_free(_nonfinite_input_perf);
 }
 
-float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
-				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+float ECL_RollController::control_attitude(float roll_setpoint, float roll)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll_setpoint) && isfinite(roll))) {
+		perf_count(_nonfinite_input_perf);
+		return _rate_setpoint;
+	}
+
+	/* Calculate error */
+	float roll_error = roll_setpoint - roll;
+
+	/* Apply P controller */
+	_rate_setpoint = roll_error / _tc;
+
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
+	return _rate_setpoint;
+}
+
+float ECL_RollController::control_bodyrate(float pitch,
+		float roll_rate, float yaw_rate,
+		float yaw_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
+			isfinite(airspeed_min) && isfinite(airspeed_max) &&
+			isfinite(scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
+
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
@@ -70,10 +114,8 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_i_rate = _k_i * _tc;
-
 	/* input conditioning */
+//	warnx("airspeed pre %.4f", (double)airspeed);
 	if (!isfinite(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
 		airspeed = 0.5f * (airspeed_min + airspeed_max);
@@ -81,32 +123,27 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 		airspeed = airspeed_min;
 	}
 
-	float roll_error = roll_setpoint - roll;
-	_rate_setpoint = roll_error / _tc;
 
-	/* limit the rate */
-	if (_max_rate > 0.01f) {
-		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
-		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
-	}
-
-	_rate_error = _rate_setpoint - roll_rate;
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
 
+	/* Transform estimation to body angular rates */
+	float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
 
-	float ilimit_scaled = _integrator_max * scaler;
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
 
-	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * k_i_rate * dt * scaler;
+		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase into the
-		 * wrong direction if actuator is at limit
-		 */
-		if (_last_output < -_max_deflection_rad) {
+		* anti-windup: do not allow integrator to increase if actuator is at limit
+		*/
+		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
-		} else if (_last_output > _max_deflection_rad) {
+		} else if (_last_output > 1.0f) {
 			/* only allow motion to center: decrease value */
 			id = math::min(id, 0.0f);
 		}
@@ -115,11 +152,14 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
-	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
 
-	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_RollController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 0d4ea9333..0799dbe03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -36,6 +36,7 @@
  * Definition of a simple orthogonal roll PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Acknowledgements:
  *
@@ -50,14 +51,21 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
-class __EXPORT ECL_RollController
+class __EXPORT ECL_RollController //XXX: create controller superclass
 {
 public:
 	ECL_RollController();
 
-	float control(float roll_setpoint, float roll, float roll_rate,
-		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+	~ECL_RollController();
+
+	float control_attitude(float roll_setpoint, float roll);
+
+	float control_bodyrate(float pitch,
+			float roll_rate, float yaw_rate,
+			float yaw_rate_setpoint,
+			float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
 
 	void reset_integrator();
 
@@ -66,18 +74,23 @@ public:
 			_tc = time_constant;
 		}
 	}
+
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate(float max_rate) {
 		_max_rate = max_rate;
 	}
@@ -90,19 +103,24 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
 	float _tc;
 	float _k_p;
 	float _k_i;
-	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate;
 	float _last_output;
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
+	perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index b786acf24..fe03b8065 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -44,29 +44,140 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
-	_last_error(0.0f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
 	_last_output(0.0f),
-	_last_rate_hp_out(0.0f),
-	_last_rate_hp_in(0.0f),
-	_k_d_last(0.0f),
-	_integrator(0.0f)
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_bodyrate_setpoint(0.0f),
+	_coordinated_min_speed(1.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
 {
+}
+
+ECL_YawController::~ECL_YawController()
+{
+	perf_free(_nonfinite_input_perf);
+}
+
+float ECL_YawController::control_attitude(float roll, float pitch,
+		float speed_body_u, float speed_body_v, float speed_body_w,
+		float roll_rate_setpoint, float pitch_rate_setpoint)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
+				isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
+				isfinite(pitch_rate_setpoint))) {
+		perf_count(_nonfinite_input_perf);
+		return _rate_setpoint;
+	}
+//	static int counter = 0;
+	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
+	_rate_setpoint = 0.0f;
+	if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
+		float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
+		if(fabsf(denumerator) > FLT_EPSILON) {
+			_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
+//			warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
+		}
 
+//		if(counter % 20 == 0) {
+//			warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
+//		}
+	}
+
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate > 0.01f) {
+	_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+	_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
+
+//	counter++;
+
+	if(!isfinite(_rate_setpoint)){
+		warnx("yaw rate sepoint not finite");
+		_rate_setpoint = 0.0f;
+	}
+
+	return _rate_setpoint;
 }
 
-float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
-				 float airspeed_min, float airspeed_max, float aspeed)
+float ECL_YawController::control_bodyrate(float roll, float pitch,
+		float pitch_rate, float yaw_rate,
+		float pitch_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
+				isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) &&
+				isfinite(airspeed_max) && isfinite(scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+	/* airspeed is NaN, +- INF or not available, pick center of band */
+	airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+	airspeed = airspeed_min;
+	}
+
+
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
+
+	/* Transform estimation to body angular rates */
+	float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
+
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+	float id = _rate_error * dt;
+
+	/*
+	 * anti-windup: do not allow integrator to increase if actuator is at limit
+	 */
+	if (_last_output < -1.0f) {
+		/* only allow motion to center: increase value */
+		id = math::max(id, 0.0f);
+	} else if (_last_output > 1.0f) {
+		/* only allow motion to center: decrease value */
+		id = math::min(id, 0.0f);
+	}
+
+	_integrator += id;
+	}
+
+	/* integrator limit */
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 
-	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
-	return 0.0f;
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_YawController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 66b227918..a360c14b8 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -35,54 +35,93 @@
  * @file ecl_yaw_controller.h
  * Definition of a simple orthogonal coordinated turn yaw PID controller.
  *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
  */
 #ifndef ECL_YAW_CONTROLLER_H
 #define ECL_YAW_CONTROLLER_H
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
-class __EXPORT ECL_YawController
+class __EXPORT ECL_YawController //XXX: create controller superclass
 {
 public:
 	ECL_YawController();
 
-	float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
-		      float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
+	~ECL_YawController();
+
+	float control_attitude(float roll, float pitch,
+			float speed_body_u, float speed_body_v, float speed_body_w,
+			float roll_rate_setpoint, float pitch_rate_setpoint);
+
+	float control_bodyrate(	float roll, float pitch,
+			float pitch_rate, float yaw_rate,
+			float pitch_rate_setpoint,
+			float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
 
 	void reset_integrator();
 
-	void set_k_side(float k_a) {
-		_k_side = k_a;
+	void set_k_p(float k_p) {
+			_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
-	}
-	void set_k_roll_ff(float k_roll_ff) {
-		_k_roll_ff = k_roll_ff;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
 
+	void set_max_rate(float max_rate) {
+		_max_rate = max_rate;
+	}
+
+	void set_coordinated_min_speed(float coordinated_min_speed) {
+		_coordinated_min_speed = coordinated_min_speed;
+	}
+
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
-
-	float _k_side;
+	float _k_p;
 	float _k_i;
-	float _k_d;
-	float _k_roll_ff;
+	float _k_ff;
 	float _integrator_max;
-
-	float _last_error;
+	float _max_rate;
+	float  _roll_ff;
 	float _last_output;
-	float _last_rate_hp_out;
-	float _last_rate_hp_in;
-	float _k_d_last;
 	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _bodyrate_setpoint;
+	float _coordinated_min_speed;
+	perf_counter_t _nonfinite_input_perf;
 
 };
 
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index e0f207696..aa3c5000a 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -38,7 +38,6 @@
  */
 
 #include <drivers/drv_hrt.h>
-#include <geo/geo.h>
 
 #define ecl_absolute_time hrt_absolute_time
-#define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file
+#define ecl_elapsed_time hrt_elapsed_time
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 27d76f959..d1c864d78 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -38,6 +38,8 @@
  *
  */
 
+#include <float.h>
+
 #include "ecl_l1_pos_controller.h"
 
 float ECL_L1_Pos_Controller::nav_roll()
@@ -70,7 +72,7 @@ float ECL_L1_Pos_Controller::target_bearing()
 float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
 {
 	/* following [2], switching on L1 distance */
-	return math::max(wp_radius, _L1_distance);
+	return math::min(wp_radius, _L1_distance);
 }
 
 bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@@ -83,8 +85,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
 	return _crosstrack_error;
 }
 
-void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
-				       const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+				       const math::Vector<2> &ground_speed_vector)
 {
 
 	/* this follows the logic presented in [1] */
@@ -94,7 +96,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float ltrack_vel;
 
 	/* get the direction between the last (visited) and next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
 
 	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@@ -103,7 +105,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	_L1_distance = _L1_ratio * ground_speed;
 
 	/* calculate vector from A to B */
-	math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+	math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
 
 	/*
 	 * check if waypoints are on top of each other. If yes,
@@ -116,7 +118,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	vector_AB.normalize();
 
 	/* calculate the vector from waypoint A to the aircraft */
-	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
 	/* calculate crosstrack error (output only) */
 	_crosstrack_error = vector_AB % vector_A_to_airplane;
@@ -130,7 +132,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
 	
 	/* calculate angle of airplane position vector relative to line) */
 
@@ -143,14 +145,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		/* calculate eta to fly to waypoint A */
 
 		/* unit vector from waypoint A to current position */
-		math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+		math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
 		/* velocity across / orthogonal to line */
 		xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
 		/* velocity along line */
 		ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 
 	/*
 	 * If the AB vector and the vector from B to airplane point in the same
@@ -174,7 +176,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
+		_nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0));
 
 	} else {
 
@@ -194,7 +196,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		float eta1 = asinf(sine_eta1);
 		eta = eta1 + eta2;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
+		_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
 
 	}
 
@@ -209,8 +211,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	_bearing_error = eta;
 }
 
-void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
-				       const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+				       const math::Vector<2> &ground_speed_vector)
 {
 	/* the complete guidance logic in this section was proposed by [2] */
 
@@ -220,7 +222,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	float K_velocity = 2.0f * _L1_damping * omega;
 
 	/* update bearing to next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
 
 	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
@@ -229,10 +231,17 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	_L1_distance = _L1_ratio * ground_speed;
 
 	/* calculate the vector from waypoint A to current position */
-	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
-	/* store the normalized vector from waypoint A to current position */
-	math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+	math::Vector<2> vector_A_to_airplane_unit;
+
+	/* prevent NaN when normalizing */
+	if (vector_A_to_airplane.length() > FLT_EPSILON) {
+		/* store the normalized vector from waypoint A to current position */
+		vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+	} else {
+		vector_A_to_airplane_unit = vector_A_to_airplane;
+	}
 
 	/* calculate eta angle towards the loiter center */
 
@@ -280,26 +289,26 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	 */
 
 	// XXX check switch over
-	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
 		(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
 		_lateral_accel = lateral_accel_sp_center;
 		_circle_mode = false;
 		/* angle between requested and current velocity vector */
 		_bearing_error = eta;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 
 	} else {
 		_lateral_accel = lateral_accel_sp_circle;
 		_circle_mode = true;
 		_bearing_error = 0.0f;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 	}
 }
 
 
-void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
 {
 	/* the complete guidance logic in this section was proposed by [2] */
 
@@ -352,14 +361,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
 }
 
 
-math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
+math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
 {
 	/* this is an approximation for small angles, proposed by [2] */
 
-	math::Vector2f out;
-
-	out.setX(math::radians((target.getX() - origin.getX())));
-	out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
+	math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
 
 	return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
 }
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h
index 7a3c42a92..5c0804a39 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -160,8 +160,8 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
-			   const math::Vector2f &ground_speed);
+	void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+			   const math::Vector<2> &ground_speed);
 
 
 	/**
@@ -172,8 +172,8 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
-			   const math::Vector2f &ground_speed_vector);
+	void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+			   const math::Vector<2> &ground_speed_vector);
 
 
 	/**
@@ -185,7 +185,7 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
 
 
 	/**
@@ -260,7 +260,7 @@ private:
 	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
 	 * @return The vector in meters pointing from the reference position to the coordinates
 	 */
-	math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
+	math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
 
 };
 
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 1d5c85699..6386e37a0 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -2,13 +2,11 @@
 
 #include "tecs.h"
 #include <ecl/ecl.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
 
 using namespace math;
 
-#ifndef CONSTANTS_ONE_G
-#define CONSTANTS_ONE_G GRAVITY
-#endif
-
 /**
  * @file tecs.cpp
  *
@@ -29,7 +27,7 @@ using namespace math;
  *
  */
 
-void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
+void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
 {
 	// Implement third order complementary filter for height and height rate
 	// estimted height rate = _integ2_state
@@ -168,64 +166,88 @@ void TECS::_update_speed_demand(void)
 	// calculate velocity rate limits based on physical performance limits
 	// provision to use a different rate limit if bad descent or underspeed condition exists
 	// Use 50% of maximum energy rate to allow margin for total energy contgroller
-	float velRateMax;
-	float velRateMin;
-
-	if ((_badDescent) || (_underspeed)) {
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-
-	} else {
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-	}
-
-	// Apply rate limit
-	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
-		_TAS_rate_dem = velRateMax;
-
-	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
-		_TAS_rate_dem = velRateMin;
-
-	} else {
-		_TAS_dem_adj = _TAS_dem;
-		_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
-	}
+//	float velRateMax;
+//	float velRateMin;
+//
+//	if ((_badDescent) || (_underspeed)) {
+//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+//
+//	} else {
+//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+//	}
+//
+//	// Apply rate limit
+//	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+//		_TAS_rate_dem = velRateMax;
+//
+//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+//		_TAS_rate_dem = velRateMin;
+//
+//	} else {
+//		_TAS_dem_adj = _TAS_dem;
+//
+//
+//	_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+//	}
+
+	_TAS_dem_adj = _TAS_dem;
+	_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
 
 	// Constrain speed demand again to protect against bad values on initialisation.
 	_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
-	_TAS_dem_last = _TAS_dem;
+//	_TAS_dem_last = _TAS_dem;
+
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f,  _badDescent %u , _underspeed %u",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state,  _badDescent , _underspeed);
 }
 
-void TECS::_update_height_demand(float demand)
+void TECS::_update_height_demand(float demand, float state)
 {
-	// Apply 2 point moving average to demanded height
-	// This is required because height demand is only updated at 5Hz
-	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
-	_hgt_dem_in_old = _hgt_dem;
-
-	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
-	// 	_maxClimbRate);
+//	// Apply 2 point moving average to demanded height
+//	// This is required because height demand is only updated at 5Hz
+//	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
+//	_hgt_dem_in_old = _hgt_dem;
+//
+//	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
+//	// 	_maxClimbRate);
+//
+//	// Limit height rate of change
+//	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
+//		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+//
+//	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
+//		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+//	}
+//
+//	_hgt_dem_prev = _hgt_dem;
+//
+//	// Apply first order lag to height demand
+//	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
+//	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
+//	_hgt_dem_adj_last = _hgt_dem_adj;
+//
+//	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
+//	// 	_hgt_rate_dem);
+
+	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
+	_hgt_dem_adj_last = _hgt_dem_adj;
 
+	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
 	// Limit height rate of change
-	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
-		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+	if (_hgt_rate_dem > _maxClimbRate) {
+		_hgt_rate_dem = _maxClimbRate;
 
-	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
-		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+	} else if (_hgt_rate_dem < -_maxSinkRate) {
+		_hgt_rate_dem = -_maxSinkRate;
 	}
 
-	_hgt_dem_prev = _hgt_dem;
-
-	// Apply first order lag to height demand
-	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
-	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
-	_hgt_dem_adj_last = _hgt_dem_adj;
-
-	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
-	// 	_hgt_rate_dem);
+	//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
 }
 
 void TECS::_detect_underspeed(void)
@@ -257,7 +279,7 @@ void TECS::_update_energies(void)
 	_SKEdot = _integ5_state * _vel_dot;
 }
 
-void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
+void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat)
 {
 	// Calculate total energy values
 	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
@@ -285,10 +307,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
 		// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
 		// drag increase during turns.
 		float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
-		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
+		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
 
 		if (STEdot_dem >= 0) {
-			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
+			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr);
 
 		} else {
 			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
@@ -353,14 +375,18 @@ void TECS::_detect_bad_descent(void)
 	// 1) Underspeed protection not active
 	// 2) Specific total energy error > 0
 	// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
-	float STEdot = _SPEdot + _SKEdot;
-
-	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
-		_badDescent = true;
-
-	} else {
-		_badDescent = false;
-	}
+//	float STEdot = _SPEdot + _SKEdot;
+//
+//	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+//
+//		warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
+//		_badDescent = true;
+//
+//	} else {
+//		_badDescent = false;
+//	}
+
+	_badDescent = false;
 }
 
 void TECS::_update_pitch(void)
@@ -476,7 +502,7 @@ void TECS::_update_STE_rate_lim(void)
 	_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
 }
 
-void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
 				 float throttle_min, float throttle_max, float throttle_cruise,
 				 float pitch_limit_min, float pitch_limit_max)
 {
@@ -508,7 +534,7 @@ void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float bar
 	_update_speed_demand();
 
 	// Calculate the height demand
-	_update_height_demand(hgt_dem);
+	_update_height_demand(hgt_dem, baro_altitude);
 
 	// Detect underspeed condition
 	_detect_underspeed();
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index f8f832ed7..5cafb1c79 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -28,16 +28,7 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
-
-		_airspeed_enabled(false),
-		_throttle_slewrate(0.0f),
-		_climbOutDem(false),
-		_hgt_dem_prev(0.0f),
-		_hgt_dem_adj_last(0.0f),
-		_hgt_dem_in_old(0.0f),
-		_TAS_dem_last(0.0f),
-		_TAS_dem_adj(0.0f),
-		_TAS_dem(0.0f),
+		_pitch_dem(0.0f),
 		_integ1_state(0.0f),
 		_integ2_state(0.0f),
 		_integ3_state(0.0f),
@@ -45,8 +36,16 @@ public:
 		_integ5_state(0.0f),
 		_integ6_state(0.0f),
 		_integ7_state(0.0f),
-		_pitch_dem(0.0f),
 		_last_pitch_dem(0.0f),
+		_vel_dot(0.0f),
+		_TAS_dem(0.0f),
+		_TAS_dem_last(0.0f),
+		_hgt_dem_in_old(0.0f),
+		_hgt_dem_adj_last(0.0f),
+		_hgt_dem_prev(0.0f),
+		_TAS_dem_adj(0.0f),
+		_STEdotErrLast(0.0f),
+		_climbOutDem(false),
 		_SPE_dem(0.0f),
 		_SKE_dem(0.0f),
 		_SPEdot_dem(0.0f),
@@ -55,9 +54,9 @@ public:
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
-		_vel_dot(0.0f),
-		_STEdotErrLast(0.0f) {
-
+		_airspeed_enabled(false),
+		_throttle_slewrate(0.0f)
+	{
 	}
 
 	bool airspeed_sensor_enabled() {
@@ -71,10 +70,10 @@ public:
 	// Update of the estimated height and height rate internal state
 	// Update of the inertial speed rate internal state
 	// Should be called at 50Hz or greater
-	void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
+	void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
 
 	// Update the control loop calculations
-	void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+	void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
 				   float throttle_min, float throttle_max, float throttle_cruise,
 				   float pitch_limit_min, float pitch_limit_max);
 	// demanded throttle in percentage
@@ -180,6 +179,14 @@ public:
 		_indicated_airspeed_max = airspeed;
 	}
 
+	void set_heightrate_p(float heightrate_p) {
+		_heightrate_p = heightrate_p;
+	}
+
+	void set_speedrate_p(float speedrate_p) {
+		_speedrate_p = speedrate_p;
+	}
+
 private:
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
@@ -203,6 +210,8 @@ private:
 	float _vertAccLim;
 	float _rollComp;
 	float _spdWeight;
+	float _heightrate_p;
+	float _speedrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
 	float _throttle_dem;
@@ -329,7 +338,7 @@ private:
 	void _update_speed_demand(void);
 
 	// Update the demanded height
-	void _update_height_demand(float demand);
+	void _update_height_demand(float demand, float state);
 
 	// Detect an underspeed condition
 	void _detect_underspeed(void);
@@ -338,7 +347,7 @@ private:
 	void _update_energies(void);
 
 	// Update Demanded Throttle
-	void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
+	void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat);
 
 	// Detect Bad Descent
 	void _detect_bad_descent(void);
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 43105fdba..e600976ce 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Lorenz Meier <lm@inf.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
@@ -42,6 +39,7 @@
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <geo/geo.h>
@@ -52,125 +50,58 @@
 #include <math.h>
 #include <stdbool.h>
 
+/*
+ * Azimuthal Equidistant Projection
+ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
+ */
 
-/* values for map projection */
-static double phi_1;
-static double sin_phi_1;
-static double cos_phi_1;
-static double lambda_0;
-static double scale;
-
-__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
-	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	phi_1 = lat_0 / 180.0 * M_PI;
-	lambda_0 = lon_0 / 180.0 * M_PI;
-
-	sin_phi_1 = sin(phi_1);
-	cos_phi_1 = cos(phi_1);
-
-	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
-
-	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-	const double r_earth = 6371000;
-
-	double lat1 = phi_1;
-	double lon1 = lambda_0;
-
-	double lat2 = phi_1 + 0.5 / 180 * M_PI;
-	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
-	double sin_lat_2 = sin(lat2);
-	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
-
-	/* 2) calculate distance rho on plane */
-	double k_bar = 0;
-	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
-	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
-	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
-
-	scale = d / rho;
+	ref->lat = lat_0 / 180.0 * M_PI;
+	ref->lon = lon_0 / 180.0 * M_PI;
 
+	ref->sin_lat = sin(ref->lat);
+	ref->cos_lat = cos(ref->lat);
 }
 
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
+__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
 {
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	double phi = lat / 180.0 * M_PI;
-	double lambda = lon / 180.0 * M_PI;
-
-	double sin_phi = sin(phi);
-	double cos_phi = cos(phi);
-
-	double k_bar = 0;
-	/* using small angle approximation (formula in comment is without aproximation) */
-	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
+	double lat_rad = lat / 180.0 * M_PI;
+	double lon_rad = lon / 180.0 * M_PI;
 
-	if (0 != c)
-		k_bar = c / sin(c);
+	double sin_lat = sin(lat_rad);
+	double cos_lat = cos(lat_rad);
+	double cos_d_lon = cos(lon_rad - ref->lon);
 
-	/* using small angle approximation (formula in comment is without aproximation) */
-	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
-	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
+	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
+	double k = (c == 0.0) ? 1.0 : (c / sin(c));
 
-//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
+	*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
+	*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
 }
 
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
+__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
 {
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-
-	double x_descaled = x / scale;
-	double y_descaled = y / scale;
-
-	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
+	double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
+	double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
+	double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
 	double sin_c = sin(c);
 	double cos_c = cos(c);
 
-	double lat_sphere = 0;
-
-	if (c != 0)
-		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
-	else
-		lat_sphere = asin(cos_c * sin_phi_1);
-
-//	printf("lat_sphere = %.10f\n",lat_sphere);
-
-	double lon_sphere = 0;
-
-	if (phi_1  == M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
+	double lat_rad;
+	double lon_rad;
 
-	} else if (phi_1 == -M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
+	if (c != 0.0) {
+		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
+		lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
 
 	} else {
-
-		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
-		//using small angle approximation
-//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
-//    	if(denominator != 0)
-//    	{
-//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
-//    	}
-//    	else
-//    	{
-//    	...
-//    	}
+		lat_rad = ref->lat;
+		lon_rad = ref->lon;
 	}
 
-//	printf("lon_sphere = %.10f\n",lon_sphere);
-
-	*lat = lat_sphere * 180.0 / M_PI;
-	*lon = lon_sphere * 180.0 / M_PI;
-
+	*lat = lat_rad * 180.0 / M_PI;
+	*lon = lon_rad * 180.0 / M_PI;
 }
 
 
@@ -188,8 +119,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
 	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
 
-	const double radius_earth = 6371000.0d;
-	return radius_earth * c; 
+	return CONSTANTS_RADIUS_OF_EARTH * c;
 }
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
@@ -199,7 +129,6 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	double lat_next_rad = lat_next * M_DEG_TO_RAD;
 	double lon_next_rad = lon_next * M_DEG_TO_RAD;
 
-	double d_lat = lat_next_rad - lat_now_rad;
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
@@ -210,22 +139,21 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	return theta;
 }
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
 	double lat_next_rad = lat_next * M_DEG_TO_RAD;
 	double lon_next_rad = lon_next * M_DEG_TO_RAD;
 
-	double d_lat = lat_next_rad - lat_now_rad;
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
-	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
+	*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+	*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
 }
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -236,13 +164,22 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
-	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
+	*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
+	*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
+}
+
+__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+
+	*lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
+	*lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
 }
 
 // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
 
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
+__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
 {
 // This function returns the distance to the nearest point on the track line.  Distance is positive if current
 // position is right of the track and negative if left of the track as seen from a point on the track line
@@ -259,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 	crosstrack_error->bearing = 0.0f;
 
 	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value;
+	if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
 
 	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -274,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 	}
 
 	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-	crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
+	crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
 
 	if (sin(bearing_diff) >= 0) {
 		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
@@ -290,8 +227,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 }
 
 
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
-		float radius, float arc_start_bearing, float arc_sweep)
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+				 float radius, float arc_start_bearing, float arc_sweep)
 {
 	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
 	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
@@ -310,29 +247,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 	crosstrack_error->bearing = 0.0f;
 
 	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
+	if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
 
 
 	if (arc_sweep >= 0) {
 		bearing_sector_start = arc_start_bearing;
 		bearing_sector_end = arc_start_bearing + arc_sweep;
 
-		if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
+		if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; }
 
 	} else {
 		bearing_sector_end = arc_start_bearing;
 		bearing_sector_start = arc_start_bearing - arc_sweep;
 
-		if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F;
+		if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; }
 	}
 
 	in_sector = false;
 
 	// Case where sector does not span zero
-	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; }
 
 	// Case where sector does span zero
-	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
+	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; }
 
 	// If in the sector then calculate distance and bearing to closest point
 	if (in_sector) {
@@ -357,23 +294,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 		// calculate the position of the start and end points.  We should not be doing this often
 		// as this function generally will not be called repeatedly when we are out of the sector.
 
-		// TO DO - this is messed up and won't compile
-		float start_disp_x = radius * sin(arc_start_bearing);
-		float start_disp_y = radius * cos(arc_start_bearing);
-		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
-		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
-		float lon_start = lon_now + start_disp_x / 111111.0d;
-		float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
-		float lon_end = lon_now + end_disp_x / 111111.0d;
-		float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
-		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+		double start_disp_x = (double)radius * sin(arc_start_bearing);
+		double start_disp_y = (double)radius * cos(arc_start_bearing);
+		double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
+		double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
+		double lon_start = lon_now + start_disp_x / 111111.0;
+		double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
+		double lon_end = lon_now + end_disp_x / 111111.0;
+		double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0;
+		double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+		double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 
 
 		if (dist_to_start < dist_to_end) {
 			crosstrack_error->distance = dist_to_start;
 			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-
 		} else {
 			crosstrack_error->past_end = true;
 			crosstrack_error->distance = dist_to_end;
@@ -382,30 +317,73 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 
 	}
 
-	crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
+	crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
 	return_value = OK;
 	return return_value;
 }
 
+__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
+		double lat_next, double lon_next, float alt_next,
+		float *dist_xy, float *dist_z)
+{
+	double current_x_rad = lat_next / 180.0 * M_PI;
+	double current_y_rad = lon_next / 180.0 * M_PI;
+	double x_rad = lat_now / 180.0 * M_PI;
+	double y_rad = lon_now / 180.0 * M_PI;
+
+	double d_lat = x_rad - current_x_rad;
+	double d_lon = y_rad - current_y_rad;
+
+	double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad);
+	double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+
+	float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
+	float dz = alt_now - alt_next;
+
+	*dist_xy = fabsf(dxy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dxy * dxy + dz * dz);
+}
+
+
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+		float x_next, float y_next, float z_next,
+		float *dist_xy, float *dist_z)
+{
+	float dx = x_now - x_next;
+	float dy = y_now - y_next;
+	float dz = z_now - z_next;
+
+	*dist_xy = sqrtf(dx * dx + dy * dy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dx * dx + dy * dy + dz * dz);
+}
+
 __EXPORT float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing) || bearing == 0) {
+	if (!isfinite(bearing)) {
 		return bearing;
 	}
 
 	int c = 0;
-
-	while (bearing > M_PI_F && c < 30) {
+	while (bearing >= M_PI_F) {
 		bearing -= M_TWOPI_F;
-		c++;
+
+		if (c++ > 3) {
+			return NAN;
+		}
 	}
 
 	c = 0;
-
-	while (bearing <=  -M_PI_F && c < 30) {
+	while (bearing < -M_PI_F) {
 		bearing += M_TWOPI_F;
-		c++;
+
+		if (c++ > 3) {
+			return NAN;
+		}
 	}
 
 	return bearing;
@@ -418,12 +396,22 @@ __EXPORT float _wrap_2pi(float bearing)
 		return bearing;
 	}
 
+	int c = 0;
 	while (bearing >= M_TWOPI_F) {
-		bearing = bearing - M_TWOPI_F;
+		bearing -= M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
 	}
 
-	while (bearing <  0.0f) {
-		bearing = bearing + M_TWOPI_F;
+	c = 0;
+	while (bearing < 0.0f) {
+		bearing += M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
 	}
 
 	return bearing;
@@ -436,12 +424,22 @@ __EXPORT float _wrap_180(float bearing)
 		return bearing;
 	}
 
-	while (bearing > 180.0f) {
-		bearing = bearing - 360.0f;
+	int c = 0;
+	while (bearing >= 180.0f) {
+		bearing -= 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
 	}
 
-	while (bearing <=  -180.0f) {
-		bearing = bearing + 360.0f;
+	c = 0;
+	while (bearing < -180.0f) {
+		bearing += 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
 	}
 
 	return bearing;
@@ -454,15 +452,23 @@ __EXPORT float _wrap_360(float bearing)
 		return bearing;
 	}
 
+	int c = 0;
 	while (bearing >= 360.0f) {
-		bearing = bearing - 360.0f;
+		bearing -= 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
 	}
 
-	while (bearing <  0.0f) {
-		bearing = bearing + 360.0f;
+	c = 0;
+	while (bearing < 0.0f) {
+		bearing += 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
 	}
 
 	return bearing;
 }
-
-
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 123ff80f1..8b286af36 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Lorenz Meier <lm@inf.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
@@ -42,13 +39,19 @@
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
 
 #pragma once
 
+#include "uORB/topics/fence.h"
+#include "uORB/topics/vehicle_global_position.h"
+
 __BEGIN_DECLS
 
+#include "geo_lookup/geo_mag_declination.h"
+
 #include <stdbool.h>
 
 #define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
@@ -64,6 +67,14 @@ struct crosstrack_error_s {
 	float bearing;		// Bearing in radians to closest point on line/arc
 } ;
 
+/* lat/lon are in radians */
+struct map_projection_reference_s {
+	double lat;
+	double lon;
+	double sin_lat;
+	double cos_lat;
+};
+
 /**
  * Initializes the map transformation.
  *
@@ -71,7 +82,7 @@ struct crosstrack_error_s {
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT void map_projection_init(double lat_0, double lon_0);
+__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
 
 /**
  * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
@@ -80,7 +91,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0);
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
+__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
 
 /**
  * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
@@ -90,7 +101,7 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
+__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
 
 /**
  * Returns the distance to the next waypoint in meters.
@@ -112,14 +123,30 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
  */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
+
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
 
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
+__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
-		float radius, float arc_start_bearing, float arc_sweep);
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+				 float radius, float arc_start_bearing, float arc_sweep);
+
+/*
+ * Calculate distance in global frame
+ */
+__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
+		double lat_next, double lon_next, float alt_next,
+		float *dist_xy, float *dist_z);
+
+/*
+ * Calculate distance in local frame (NED)
+ */
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+		float x_next, float y_next, float z_next,
+		float *dist_xy, float *dist_z);
 
 __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
index 30a2dc99f..d08ca4532 100644
--- a/src/lib/geo/module.mk
+++ b/src/lib/geo/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
@@ -35,4 +35,4 @@
 # Geo library
 #
 
-SRCS		 = geo.c
+SRCS		=	geo.c
diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c
new file mode 100644
index 000000000..c41d52606
--- /dev/null
+++ b/src/lib/geo_lookup/geo_mag_declination.c
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.c
+*
+* Calculation / lookup table for earth magnetic field declination.
+* 
+* Lookup table from Scott Ferguson <scottfromscott@gmail.com>
+*
+* XXX Lookup table currently too coarse in resolution (only full degrees)
+* and lat/lon res - needs extension medium term.
+*
+*/
+
+#include <geo/geo.h>
+
+/** set this always to the sampling in degrees for the table below */
+#define SAMPLING_RES		10.0f
+#define SAMPLING_MIN_LAT	-60.0f
+#define SAMPLING_MAX_LAT	60.0f
+#define SAMPLING_MIN_LON	-180.0f
+#define SAMPLING_MAX_LON	180.0f
+
+static const int8_t declination_table[13][37] = \
+{
+	{ 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 },
+    { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 },
+    { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 },
+    { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
+    { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 },
+    { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 },
+    { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 },
+    { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 },
+    { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 },
+    { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 },
+    { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 },
+    { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 },
+    { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 },
+};
+
+static float get_lookup_table_val(unsigned lat, unsigned lon);
+
+__EXPORT float get_mag_declination(float lat, float lon)
+{
+	/*
+	 * If the values exceed valid ranges, return zero as default
+	 * as we have no way of knowing what the closest real value
+	 * would be.
+	 */
+	if (lat < -90.0f || lat > 90.0f ||
+	    lon < -180.0f || lon > 180.0f) {
+		return 0.0f;
+	}
+
+	/* round down to nearest sampling resolution */
+	int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
+	int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
+
+	/* for the rare case of hitting the bounds exactly
+	 * the rounding logic wouldn't fit, so enforce it.
+	 */
+
+	/* limit to table bounds - required for maxima even when table spans full globe range */
+	if (lat <= SAMPLING_MIN_LAT) {
+		min_lat = SAMPLING_MIN_LAT;
+	}
+
+	if (lat >= SAMPLING_MAX_LAT) {
+		min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
+	}
+
+	if (lon <= SAMPLING_MIN_LON) {
+		min_lon = SAMPLING_MIN_LON;
+	}
+
+	if (lon >= SAMPLING_MAX_LON) {
+		min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
+	}
+
+	/* find index of nearest low sampling point */
+	unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat)  / SAMPLING_RES;
+	unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
+
+	float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
+	float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
+	float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
+	float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
+
+	/* perform bilinear interpolation on the four grid corners */
+
+	float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
+	float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
+
+	return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
+}
+
+float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
+{
+	return declination_table[lat_index][lon_index];
+}
\ No newline at end of file
diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/geo_lookup/geo_mag_declination.h
index 7ea6496bb..0ac062d6d 100644
--- a/src/lib/mathlib/math/arm/Vector.cpp
+++ b/src/lib/geo_lookup/geo_mag_declination.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name MAVGEO nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -32,9 +32,16 @@
  ****************************************************************************/
 
 /**
- * @file Vector.cpp
- *
- * math vector
- */
+* @file geo_mag_declination.h
+*
+* Calculation / lookup table for earth magnetic field declination.
+*
+*/
+
+#pragma once
+
+__BEGIN_DECLS
+
+__EXPORT float get_mag_declination(float lat, float lon);
 
-#include "Vector.hpp"
+__END_DECLS
diff --git a/src/examples/flow_position_control/module.mk b/src/lib/geo_lookup/module.mk
index b10dc490a..d7a10df2d 100644
--- a/src/examples/flow_position_control/module.mk
+++ b/src/lib/geo_lookup/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   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
@@ -32,10 +32,9 @@
 ############################################################################
 
 #
-# Build multirotor position control
+# Geo lookup table / data library
 #
 
-MODULE_COMMAND	= flow_position_control
+SRCS		 =	geo_mag_declination.c
 
-SRCS		= flow_position_control_main.c \
-		  flow_position_control_params.c
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
new file mode 100644
index 000000000..c555a0a69
--- /dev/null
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   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 CatapultLaunchMethod.cpp
+ * Catapult Launch detection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ */
+
+#include "CatapultLaunchMethod.h"
+#include <systemlib/err.h>
+
+namespace launchdetection
+{
+
+CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
+	SuperBlock(parent, "CAT"),
+	last_timestamp(hrt_absolute_time()),
+	integrator(0.0f),
+	launchDetected(false),
+	threshold_accel(this, "A"),
+	threshold_time(this, "T")
+{
+
+}
+
+CatapultLaunchMethod::~CatapultLaunchMethod() {
+
+}
+
+void CatapultLaunchMethod::update(float accel_x)
+{
+	float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
+	last_timestamp = hrt_absolute_time();
+
+	if (accel_x > threshold_accel.get()) {
+		integrator += accel_x * dt;
+//		warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
+//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
+		if (integrator > threshold_accel.get() * threshold_time.get()) {
+			launchDetected = true;
+		}
+
+	} else {
+//		warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
+//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
+		/* reset integrator */
+		integrator = 0.0f;
+		launchDetected = false;
+	}
+
+}
+
+bool CatapultLaunchMethod::getLaunchDetected()
+{
+	return launchDetected;
+}
+
+
+void CatapultLaunchMethod::reset()
+{
+	integrator = 0.0f;
+	launchDetected = false;
+}
+
+}
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
new file mode 100644
index 000000000..23757f6f3
--- /dev/null
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   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 CatapultLaunchMethod.h
+ * Catpult Launch detection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef CATAPULTLAUNCHMETHOD_H_
+#define CATAPULTLAUNCHMETHOD_H_
+
+#include "LaunchMethod.h"
+
+#include <drivers/drv_hrt.h>
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+namespace launchdetection
+{
+
+class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
+{
+public:
+	CatapultLaunchMethod(SuperBlock *parent);
+	~CatapultLaunchMethod();
+
+	void update(float accel_x);
+	bool getLaunchDetected();
+	void reset();
+
+private:
+	hrt_abstime last_timestamp;
+	float integrator;
+	bool launchDetected;
+
+	control::BlockParamFloat threshold_accel;
+	control::BlockParamFloat threshold_time;
+
+};
+
+#endif /* CATAPULTLAUNCHMETHOD_H_ */
+
+}
diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index 35158a396..3bf47bbb0 100644
--- a/src/lib/mathlib/math/Vector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -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
@@ -30,71 +30,64 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 /**
- * @file Vector.cpp
+ * @file launchDetection.cpp
+ * Auto Detection for different launch methods (e.g. catapult)
  *
- * math vector
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include "test/test.hpp"
-
-#include "Vector.hpp"
+#include "LaunchDetector.h"
+#include "CatapultLaunchMethod.h"
+#include <systemlib/err.h>
 
-namespace math
+namespace launchdetection
 {
 
-static const float data_testA[] = {1, 3};
-static const float data_testB[] = {4, 1};
+LaunchDetector::LaunchDetector() :
+	SuperBlock(NULL, "LAUN"),
+	launchdetection_on(this, "ALL_ON"),
+	throttlePreTakeoff(this, "THR_PRE")
+{
+	/* init all detectors */
+	launchMethods[0] = new CatapultLaunchMethod(this);
 
-static Vector testA(2, data_testA);
-static Vector testB(2, data_testB);
 
-int __EXPORT vectorTest()
-{
-	vectorAddTest();
-	vectorSubTest();
-	return 0;
+	/* update all parameters of all detectors */
+	updateParams();
 }
 
-int vectorAddTest()
+LaunchDetector::~LaunchDetector()
 {
-	printf("Test Vector Add\t\t: ");
-	Vector r = testA + testB;
-	float data_test[] = {5.0f, 4.0f};
-	ASSERT(vectorEqual(Vector(2, data_test), r));
-	printf("PASS\n");
-	return 0;
+
 }
 
-int vectorSubTest()
+void LaunchDetector::reset()
 {
-	printf("Test Vector Sub\t\t: ");
-	Vector r(2);
-	r = testA - testB;
-	float data_test[] = { -3.0f, 2.0f};
-	ASSERT(vectorEqual(Vector(2, data_test), r));
-	printf("PASS\n");
-	return 0;
+	/* Reset all detectors */
+	launchMethods[0]->reset();
 }
 
-bool vectorEqual(const Vector &a, const Vector &b, float eps)
+void LaunchDetector::update(float accel_x)
 {
-	if (a.getRows() != b.getRows()) {
-		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
-		return false;
+	if (launchdetection_on.get() == 1) {
+		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+			launchMethods[i]->update(accel_x);
+		}
 	}
+}
 
-	bool ret = true;
-
-	for (size_t i = 0; i < a.getRows(); i++) {
-		if (!equal(a(i), b(i), eps)) {
-			printf("element mismatch (%d)\n", i);
-			ret = false;
+bool LaunchDetector::getLaunchDetected()
+{
+	if (launchdetection_on.get() == 1) {
+		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+			if(launchMethods[i]->getLaunchDetected()) {
+				return true;
+			}
 		}
 	}
 
-	return ret;
+	return false;
 }
 
-} // namespace math
+}
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
new file mode 100644
index 000000000..1a214b66e
--- /dev/null
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -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 LaunchDetector.h
+ * Auto Detection for different launch methods (e.g. catapult)
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef LAUNCHDETECTOR_H
+#define LAUNCHDETECTOR_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "LaunchMethod.h"
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+namespace launchdetection
+{
+
+class __EXPORT LaunchDetector : public control::SuperBlock
+{
+public:
+	LaunchDetector();
+	~LaunchDetector();
+	void reset();
+
+	void update(float accel_x);
+	bool getLaunchDetected();
+	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
+
+	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
+
+//	virtual bool getLaunchDetected();
+protected:
+private:
+	LaunchMethod* launchMethods[1];
+	control::BlockParamInt launchdetection_on;
+	control::BlockParamFloat throttlePreTakeoff;
+
+
+};
+
+}
+
+#endif // LAUNCHDETECTOR_H
diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/lib/launchdetection/LaunchMethod.h
index 21661622a..e04467f6a 100644
--- a/src/lib/mathlib/math/generic/Matrix.cpp
+++ b/src/lib/launchdetection/LaunchMethod.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
@@ -32,9 +32,29 @@
  ****************************************************************************/
 
 /**
- * @file Matrix.cpp
+ * @file LaunchMethod.h
+ * Base class for different launch methods
  *
- * matrix code
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include "Matrix.hpp"
+#ifndef LAUNCHMETHOD_H_
+#define LAUNCHMETHOD_H_
+
+namespace launchdetection
+{
+
+class LaunchMethod
+{
+public:
+	virtual void update(float accel_x) = 0;
+	virtual bool getLaunchDetected() = 0;
+	virtual void reset() = 0;
+
+protected:
+private:
+};
+
+}
+
+#endif /* LAUNCHMETHOD_H_ */
diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/lib/launchdetection/launchdetection_params.c
index 4af5edead..8df8c696c 100644
--- a/src/modules/att_pos_estimator_ekf/params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 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
@@ -31,19 +31,59 @@
  *
  ****************************************************************************/
 
+/**
+ * @file launchdetection_params.c
+ *
+ * Parameters for launchdetection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
 #include <systemlib/param/param.h>
 
-/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f);
-PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
-PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
-PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
-PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
-PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
+/*
+ * Catapult launch detection parameters, accessible via MAVLink
+ *
+ */
+
+/**
+ * Enable launch detection.
+ *
+ * @min 0
+ * @max 1
+ * @group Launch detection
+ */
+PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
+
+/**
+ * Catapult accelerometer theshold.
+ *
+ * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
+ *
+ * @min 0
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
+
+/**
+ * Catapult time theshold.
+ *
+ * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
+ *
+ * @min 0
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
+
+/**
+ * Throttle setting while detecting launch.
+ *
+ * The throttle is set to this value while the system is waiting for the take-off.
+ *
+ * @min 0
+ * @max 1
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);
diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk
new file mode 100644
index 000000000..d92f0bb45
--- /dev/null
+++ b/src/lib/launchdetection/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# Launchdetection Library
+#
+
+SRCS		 = 	LaunchDetector.cpp \
+			CatapultLaunchMethod.cpp \
+			launchdetection_params.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
index 6f66f9ee3..61d3a3b61 100644
--- a/src/lib/mathlib/CMSIS/Include/arm_math.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_math.h
@@ -5193,7 +5193,7 @@ void arm_rfft_fast_f32(
     *pIa = Ialpha;

 

     /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */

-    *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;

+    *pIb = (float32_t)-0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;

 

   }

 

diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp
deleted file mode 100644
index f509f7081..000000000
--- a/src/lib/mathlib/math/Dcm.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 Dcm.cpp
- *
- * math direction cosine matrix
- */
-
-#include <mathlib/math/test/test.hpp>
-
-#include "Dcm.hpp"
-#include "Quaternion.hpp"
-#include "EulerAngles.hpp"
-#include "Vector3.hpp"
-
-namespace math
-{
-
-Dcm::Dcm() :
-	Matrix(Matrix::identity(3))
-{
-}
-
-Dcm::Dcm(float c00, float c01, float c02,
-	 float c10, float c11, float c12,
-	 float c20, float c21, float c22) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	dcm(0, 0) = c00;
-	dcm(0, 1) = c01;
-	dcm(0, 2) = c02;
-	dcm(1, 0) = c10;
-	dcm(1, 1) = c11;
-	dcm(1, 2) = c12;
-	dcm(2, 0) = c20;
-	dcm(2, 1) = c21;
-	dcm(2, 2) = c22;
-}
-
-Dcm::Dcm(const float data[3][3]) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	/* set rotation matrix */
-	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-		dcm(i, j) = data[i][j];
-}
-
-Dcm::Dcm(const float *data) :
-	Matrix(3, 3, data)
-{
-}
-
-Dcm::Dcm(const Quaternion &q) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	double a = q.getA();
-	double b = q.getB();
-	double c = q.getC();
-	double d = q.getD();
-	double aSq = a * a;
-	double bSq = b * b;
-	double cSq = c * c;
-	double dSq = d * d;
-	dcm(0, 0) = aSq + bSq - cSq - dSq;
-	dcm(0, 1) = 2.0 * (b * c - a * d);
-	dcm(0, 2) = 2.0 * (a * c + b * d);
-	dcm(1, 0) = 2.0 * (b * c + a * d);
-	dcm(1, 1) = aSq - bSq + cSq - dSq;
-	dcm(1, 2) = 2.0 * (c * d - a * b);
-	dcm(2, 0) = 2.0 * (b * d - a * c);
-	dcm(2, 1) = 2.0 * (a * b + c * d);
-	dcm(2, 2) = aSq - bSq - cSq + dSq;
-}
-
-Dcm::Dcm(const EulerAngles &euler) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	double cosPhi = cos(euler.getPhi());
-	double sinPhi = sin(euler.getPhi());
-	double cosThe = cos(euler.getTheta());
-	double sinThe = sin(euler.getTheta());
-	double cosPsi = cos(euler.getPsi());
-	double sinPsi = sin(euler.getPsi());
-
-	dcm(0, 0) = cosThe * cosPsi;
-	dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
-	dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
-
-	dcm(1, 0) = cosThe * sinPsi;
-	dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
-	dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
-
-	dcm(2, 0) = -sinThe;
-	dcm(2, 1) = sinPhi * cosThe;
-	dcm(2, 2) = cosPhi * cosThe;
-}
-
-Dcm::Dcm(const Dcm &right) :
-	Matrix(right)
-{
-}
-
-Dcm::~Dcm()
-{
-}
-
-int __EXPORT dcmTest()
-{
-	printf("Test DCM\t\t: ");
-	// default ctor
-	ASSERT(matrixEqual(Dcm(),
-			   Matrix::identity(3)));
-	// quaternion ctor
-	ASSERT(matrixEqual(
-		       Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
-		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
-			   0.2896295f,  0.9564251f, -0.0369570f,
-			   -0.1986693f,  0.0978434f,  0.9751703f)));
-	// euler angle ctor
-	ASSERT(matrixEqual(
-		       Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
-		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
-			   0.2896295f,  0.9564251f, -0.0369570f,
-			   -0.1986693f,  0.0978434f,  0.9751703f)));
-	// rotations
-	Vector3 vB(1, 2, 3);
-	ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
-			   Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
-	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
-			   Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
-	ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
-			   Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
-	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
-			   Dcm(EulerAngles(
-				       M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
-	printf("PASS\n");
-	return 0;
-}
-} // namespace math
diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp
deleted file mode 100644
index e733d23bb..000000000
--- a/src/lib/mathlib/math/EulerAngles.cpp
+++ /dev/null
@@ -1,126 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 Vector.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-#include "EulerAngles.hpp"
-#include "Quaternion.hpp"
-#include "Dcm.hpp"
-#include "Vector3.hpp"
-
-namespace math
-{
-
-EulerAngles::EulerAngles() :
-	Vector(3)
-{
-	setPhi(0.0f);
-	setTheta(0.0f);
-	setPsi(0.0f);
-}
-
-EulerAngles::EulerAngles(float phi, float theta, float psi) :
-	Vector(3)
-{
-	setPhi(phi);
-	setTheta(theta);
-	setPsi(psi);
-}
-
-EulerAngles::EulerAngles(const Quaternion &q) :
-	Vector(3)
-{
-	(*this) = EulerAngles(Dcm(q));
-}
-
-EulerAngles::EulerAngles(const Dcm &dcm) :
-	Vector(3)
-{
-	setTheta(asinf(-dcm(2, 0)));
-
-	if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
-		setPhi(0.0f);
-		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
-			      dcm(0, 2) + dcm(1, 1)) + getPhi());
-
-	} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
-		setPhi(0.0f);
-		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
-			      dcm(0, 2) + dcm(1, 1)) - getPhi());
-
-	} else {
-		setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
-		setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
-	}
-}
-
-EulerAngles::~EulerAngles()
-{
-}
-
-int __EXPORT eulerAnglesTest()
-{
-	printf("Test EulerAngles\t: ");
-	EulerAngles euler(0.1f, 0.2f, 0.3f);
-
-	// test ctor
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-	ASSERT(equal(euler.getPhi(), 0.1f));
-	ASSERT(equal(euler.getTheta(), 0.2f));
-	ASSERT(equal(euler.getPsi(), 0.3f));
-
-	// test dcm ctor
-	euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-
-	// test quat ctor
-	euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-
-	// test assignment
-	euler.setPhi(0.4f);
-	euler.setTheta(0.5f);
-	euler.setPsi(0.6f);
-	ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
-
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp
deleted file mode 100644
index ebd1aeda3..000000000
--- a/src/lib/mathlib/math/Matrix.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 Matrix.cpp
- *
- * matrix code
- */
-
-#include "test/test.hpp"
-#include <math.h>
-
-#include "Matrix.hpp"
-
-namespace math
-{
-
-static const float data_testA[] = {
-	1, 2, 3,
-	4, 5, 6
-};
-static Matrix testA(2, 3, data_testA);
-
-static const float data_testB[] = {
-	0, 1, 3,
-	7, -1, 2
-};
-static Matrix testB(2, 3, data_testB);
-
-static const float data_testC[] = {
-	0, 1,
-	2, 1,
-	3, 2
-};
-static Matrix testC(3, 2, data_testC);
-
-static const float data_testD[] = {
-	0, 1, 2,
-	2, 1, 4,
-	5, 2, 0
-};
-static Matrix testD(3, 3, data_testD);
-
-static const float data_testE[] = {
-	1, -1, 2,
-	0, 2, 3,
-	2, -1, 1
-};
-static Matrix testE(3, 3, data_testE);
-
-static const float data_testF[] = {
-	3.777e006f, 2.915e007f, 0.000e000f,
-	2.938e007f, 2.267e008f, 0.000e000f,
-	0.000e000f, 0.000e000f, 6.033e008f
-};
-static Matrix testF(3, 3, data_testF);
-
-int __EXPORT matrixTest()
-{
-	matrixAddTest();
-	matrixSubTest();
-	matrixMultTest();
-	matrixInvTest();
-	matrixDivTest();
-	return 0;
-}
-
-int matrixAddTest()
-{
-	printf("Test Matrix Add\t\t: ");
-	Matrix r = testA + testB;
-	float data_test[] = {
-		1.0f, 3.0f, 6.0f,
-		11.0f, 4.0f, 8.0f
-	};
-	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixSubTest()
-{
-	printf("Test Matrix Sub\t\t: ");
-	Matrix r = testA - testB;
-	float data_test[] = {
-		1.0f, 1.0f, 0.0f,
-		-3.0f, 6.0f, 4.0f
-	};
-	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixMultTest()
-{
-	printf("Test Matrix Mult\t: ");
-	Matrix r = testC * testB;
-	float data_test[] = {
-		7.0f, -1.0f,  2.0f,
-		7.0f,  1.0f,  8.0f,
-		14.0f,  1.0f, 13.0f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixInvTest()
-{
-	printf("Test Matrix Inv\t\t: ");
-	Matrix origF = testF;
-	Matrix r = testF.inverse();
-	float data_test[] = {
-		-0.0012518f,  0.0001610f, 0.0000000f,
-		0.0001622f, -0.0000209f, 0.0000000f,
-		0.0000000f,  0.0000000f, 1.6580e-9f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	// make sure F in unchanged
-	ASSERT(matrixEqual(origF, testF));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixDivTest()
-{
-	printf("Test Matrix Div\t\t: ");
-	Matrix r = testD / testE;
-	float data_test[] = {
-		0.2222222f, 0.5555556f, -0.1111111f,
-		0.0f,       1.0f,         1.0,
-		-4.1111111f, 1.2222222f,  4.5555556f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
-{
-	if (a.getRows() != b.getRows()) {
-		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
-		return false;
-
-	} else if (a.getCols() != b.getCols()) {
-		printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
-		return false;
-	}
-
-	bool ret = true;
-
-	for (size_t i = 0; i < a.getRows(); i++)
-		for (size_t j = 0; j < a.getCols(); j++) {
-			if (!equal(a(i, j), b(i, j), eps)) {
-				printf("element mismatch (%d, %d)\n", i, j);
-				ret = false;
-			}
-		}
-
-	return ret;
-}
-
-} // namespace math
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index f19db15ec..ea0cf4ca1 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -1,6 +1,9 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *           Pavel Kirienko <pavel.kirienko@gmail.com>
+ *           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
@@ -32,30 +35,401 @@
  ****************************************************************************/
 
 /**
- * @file Matrix.h
+ * @file Matrix.hpp
  *
- * matrix code
+ * Matrix class
  */
 
-#pragma once
+#ifndef MATRIX_HPP
+#define MATRIX_HPP
 
-#include <nuttx/config.h>
-
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Matrix.hpp"
-#else
-#include "generic/Matrix.hpp"
-#endif
+#include <stdio.h>
+#include "../CMSIS/Include/arm_math.h"
 
 namespace math
 {
-class Matrix;
-int matrixTest();
-int matrixAddTest();
-int matrixSubTest();
-int matrixMultTest();
-int matrixInvTest();
-int matrixDivTest();
-int matrixArmTest();
-bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
-} // namespace math
+
+template <unsigned int M, unsigned int N>
+class __EXPORT Matrix;
+
+// MxN matrix with float elements
+template <unsigned int M, unsigned int N>
+class __EXPORT MatrixBase
+{
+public:
+	/**
+	 * matrix data[row][col]
+	 */
+	float data[M][N];
+
+	/**
+	 * struct for using arm_math functions
+	 */
+	arm_matrix_instance_f32 arm_mat;
+
+	/**
+	 * trivial ctor
+	 * note that this ctor will not initialize elements
+	 */
+	MatrixBase() {
+		arm_mat = {M, N, &data[0][0]};
+	}
+
+	/**
+	 * copyt ctor
+	 */
+	MatrixBase(const MatrixBase<M, N> &m) {
+		arm_mat = {M, N, &data[0][0]};
+		memcpy(data, m.data, sizeof(data));
+	}
+
+	MatrixBase(const float *d) {
+		arm_mat = {M, N, &data[0][0]};
+		memcpy(data, d, sizeof(data));
+	}
+
+	MatrixBase(const float d[M][N]) {
+		arm_mat = {M, N, &data[0][0]};
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float *d) {
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[M][N]) {
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * access by index
+	 */
+	float &operator()(const unsigned int row, const unsigned int col) {
+		return data[row][col];
+	}
+
+	/**
+	 * access by index
+	 */
+	float operator()(const unsigned int row, const unsigned int col) const {
+		return data[row][col];
+	}
+
+	/**
+	 * get rows number
+	 */
+	unsigned int get_rows() const {
+		return M;
+	}
+
+	/**
+	 * get columns number
+	 */
+	unsigned int get_cols() const {
+		return N;
+	}
+
+	/**
+	 * test for equality
+	 */
+	bool operator ==(const Matrix<M, N> &m) const {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				if (data[i][j] != m.data[i][j])
+					return false;
+
+		return true;
+	}
+
+	/**
+	 * test for inequality
+	 */
+	bool operator !=(const Matrix<M, N> &m) const {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				if (data[i][j] != m.data[i][j])
+					return true;
+
+		return false;
+	}
+
+	/**
+	 * set to value
+	 */
+	const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
+		memcpy(data, m.data, sizeof(data));
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	/**
+	 * negation
+	 */
+	Matrix<M, N> operator -(void) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			for (unsigned int j = 0; j < M; j++)
+				res.data[i][j] = -data[i][j];
+
+		return res;
+	}
+
+	/**
+	 * addition
+	 */
+	Matrix<M, N> operator +(const Matrix<M, N> &m) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			for (unsigned int j = 0; j < M; j++)
+				res.data[i][j] = data[i][j] + m.data[i][j];
+
+		return res;
+	}
+
+	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
+		for (unsigned int i = 0; i < N; i++)
+			for (unsigned int j = 0; j < M; j++)
+				data[i][j] += m.data[i][j];
+
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	/**
+	 * subtraction
+	 */
+	Matrix<M, N> operator -(const Matrix<M, N> &m) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				res.data[i][j] = data[i][j] - m.data[i][j];
+
+		return res;
+	}
+
+	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
+		for (unsigned int i = 0; i < N; i++)
+			for (unsigned int j = 0; j < M; j++)
+				data[i][j] -= m.data[i][j];
+
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	Matrix<M, N> operator *(const float num) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				res.data[i][j] = data[i][j] * num;
+
+		return res;
+	}
+
+	Matrix<M, N> &operator *=(const float num) {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				data[i][j] *= num;
+
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	Matrix<M, N> operator /(const float num) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				res[i][j] = data[i][j] / num;
+
+		return res;
+	}
+
+	Matrix<M, N> &operator /=(const float num) {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				data[i][j] /= num;
+
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	/**
+	 * multiplication by another matrix
+	 */
+	template <unsigned int P>
+	Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+		Matrix<M, P> res;
+		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
+		return res;
+	}
+
+	/**
+	 * transpose the matrix
+	 */
+	Matrix<N, M> transposed(void) const {
+		Matrix<N, M> res;
+		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
+		return res;
+	}
+
+	/**
+	 * invert the matrix
+	 */
+	Matrix<M, N> inversed(void) const {
+		Matrix<M, N> res;
+		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
+		return res;
+	}
+
+	/**
+	 * set zero matrix
+	 */
+	void zero(void) {
+		memset(data, 0, sizeof(data));
+	}
+
+	/**
+	 * set identity matrix
+	 */
+	void identity(void) {
+		memset(data, 0, sizeof(data));
+		unsigned int n = (M < N) ? M : N;
+
+		for (unsigned int i = 0; i < n; i++)
+			data[i][i] = 1;
+	}
+
+	void print(void) {
+		for (unsigned int i = 0; i < M; i++) {
+			printf("[ ");
+
+			for (unsigned int j = 0; j < N; j++)
+				printf("%.3f\t", data[i][j]);
+
+			printf(" ]\n");
+		}
+	}
+};
+
+template <unsigned int M, unsigned int N>
+class __EXPORT Matrix : public MatrixBase<M, N>
+{
+public:
+	using MatrixBase<M, N>::operator *;
+
+	Matrix() : MatrixBase<M, N>() {}
+
+	Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}
+
+	Matrix(const float *d) : MatrixBase<M, N>(d) {}
+
+	Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {}
+
+	/**
+	 * set to value
+	 */
+	const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
+		memcpy(this->data, m.data, sizeof(this->data));
+		return *this;
+	}
+
+	/**
+	 * multiplication by a vector
+	 */
+	Vector<M> operator *(const Vector<N> &v) const {
+		Vector<M> res;
+		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+		return res;
+	}
+};
+
+template <>
+class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
+{
+public:
+	using MatrixBase<3, 3>::operator *;
+
+	Matrix() : MatrixBase<3, 3>() {}
+
+	Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}
+
+	Matrix(const float *d) : MatrixBase<3, 3>(d) {}
+
+	Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {}
+
+	/**
+	 * set to value
+	 */
+	const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) {
+		memcpy(this->data, m.data, sizeof(this->data));
+		return *this;
+	}
+
+	/**
+	 * multiplication by a vector
+	 */
+	Vector<3> operator *(const Vector<3> &v) const {
+		Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
+			      data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
+			      data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
+		return res;
+	}
+
+	/**
+	 * create a rotation matrix from given euler angles
+	 * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
+	 */
+	void from_euler(float roll, float pitch, float yaw) {
+		float cp = cosf(pitch);
+		float sp = sinf(pitch);
+		float sr = sinf(roll);
+		float cr = cosf(roll);
+		float sy = sinf(yaw);
+		float cy = cosf(yaw);
+
+		data[0][0] = cp * cy;
+		data[0][1] = (sr * sp * cy) - (cr * sy);
+		data[0][2] = (cr * sp * cy) + (sr * sy);
+		data[1][0] = cp * sy;
+		data[1][1] = (sr * sp * sy) + (cr * cy);
+		data[1][2] = (cr * sp * sy) - (sr * cy);
+		data[2][0] = -sp;
+		data[2][1] = sr * cp;
+		data[2][2] = cr * cp;
+	}
+
+	/**
+	 * get euler angles from rotation matrix
+	 */
+	Vector<3> to_euler(void) const {
+		Vector<3> euler;
+		euler.data[1] = asinf(-data[2][0]);
+
+		if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
+			euler.data[0] = 0.0f;
+			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];
+
+		} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
+			euler.data[0] = 0.0f;
+			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];
+
+		} else {
+			euler.data[0] = atan2f(data[2][1], data[2][2]);
+			euler.data[2] = atan2f(data[1][0], data[0][0]);
+		}
+
+		return euler;
+	}
+};
+
+}
+
+#endif // MATRIX_HPP
diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp
deleted file mode 100644
index 02fec4ca6..000000000
--- a/src/lib/mathlib/math/Quaternion.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 Quaternion.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-
-#include "Quaternion.hpp"
-#include "Dcm.hpp"
-#include "EulerAngles.hpp"
-
-namespace math
-{
-
-Quaternion::Quaternion() :
-	Vector(4)
-{
-	setA(1.0f);
-	setB(0.0f);
-	setC(0.0f);
-	setD(0.0f);
-}
-
-Quaternion::Quaternion(float a, float b,
-		       float c, float d) :
-	Vector(4)
-{
-	setA(a);
-	setB(b);
-	setC(c);
-	setD(d);
-}
-
-Quaternion::Quaternion(const float *data) :
-	Vector(4, data)
-{
-}
-
-Quaternion::Quaternion(const Vector &v) :
-	Vector(v)
-{
-}
-
-Quaternion::Quaternion(const Dcm &dcm) :
-	Vector(4)
-{
-	// avoiding singularities by not using
-	// division equations
-	setA(0.5 * sqrt(1.0 +
-			double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
-	setB(0.5 * sqrt(1.0 +
-			double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
-	setC(0.5 * sqrt(1.0 +
-			double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
-	setD(0.5 * sqrt(1.0 +
-			double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
-}
-
-Quaternion::Quaternion(const EulerAngles &euler) :
-	Vector(4)
-{
-	double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
-	double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
-	double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
-	double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
-	double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
-	double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
-	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
-	     sinPhi_2 * sinTheta_2 * sinPsi_2);
-	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
-	     cosPhi_2 * sinTheta_2 * sinPsi_2);
-	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
-	     sinPhi_2 * cosTheta_2 * sinPsi_2);
-	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
-	     sinPhi_2 * sinTheta_2 * cosPsi_2);
-}
-
-Quaternion::Quaternion(const Quaternion &right) :
-	Vector(right)
-{
-}
-
-Quaternion::~Quaternion()
-{
-}
-
-Vector Quaternion::derivative(const Vector &w)
-{
-#ifdef QUATERNION_ASSERT
-	ASSERT(w.getRows() == 3);
-#endif
-	float dataQ[] = {
-		getA(), -getB(), -getC(), -getD(),
-		getB(),  getA(), -getD(),  getC(),
-		getC(),  getD(),  getA(), -getB(),
-		getD(), -getC(),  getB(),  getA()
-	};
-	Vector v(4);
-	v(0) = 0.0f;
-	v(1) = w(0);
-	v(2) = w(1);
-	v(3) = w(2);
-	Matrix Q(4, 4, dataQ);
-	return Q * v * 0.5f;
-}
-
-int __EXPORT quaternionTest()
-{
-	printf("Test Quaternion\t\t: ");
-	// test default ctor
-	Quaternion q;
-	ASSERT(equal(q.getA(), 1.0f));
-	ASSERT(equal(q.getB(), 0.0f));
-	ASSERT(equal(q.getC(), 0.0f));
-	ASSERT(equal(q.getD(), 0.0f));
-	// test float ctor
-	q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
-	ASSERT(equal(q.getA(), 0.1825742f));
-	ASSERT(equal(q.getB(), 0.3651484f));
-	ASSERT(equal(q.getC(), 0.5477226f));
-	ASSERT(equal(q.getD(), 0.7302967f));
-	// test euler ctor
-	q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
-	// test dcm ctor
-	q = Quaternion(Dcm());
-	ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
-	// TODO test derivative
-	// test accessors
-	q.setA(0.1f);
-	q.setB(0.2f);
-	q.setC(0.3f);
-	q.setD(0.4f);
-	ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 048a55d33..21d05c7ef 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -1,6 +1,9 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *           Pavel Kirienko <pavel.kirienko@gmail.com>
+ *           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
@@ -34,82 +37,129 @@
 /**
  * @file Quaternion.hpp
  *
- * math quaternion lib
+ * Quaternion class
  */
 
-#pragma once
+#ifndef QUATERNION_HPP
+#define QUATERNION_HPP
 
+#include <math.h>
+#include "../CMSIS/Include/arm_math.h"
 #include "Vector.hpp"
 #include "Matrix.hpp"
 
 namespace math
 {
 
-class Dcm;
-class EulerAngles;
-
-class __EXPORT Quaternion : public Vector
+class __EXPORT Quaternion : public Vector<4>
 {
 public:
-
 	/**
-	 * default ctor
+	 * trivial ctor
 	 */
-	Quaternion();
+	Quaternion() : Vector<4>() {}
 
 	/**
-	 * ctor from floats
+	 * copy ctor
 	 */
-	Quaternion(float a, float b, float c, float d);
+	Quaternion(const Quaternion &q) : Vector<4>(q) {}
 
 	/**
-	 * ctor from data
+	 * casting from vector
 	 */
-	Quaternion(const float *data);
+	Quaternion(const Vector<4> &v) : Vector<4>(v) {}
 
 	/**
-	 * ctor from Vector
+	 * setting ctor
 	 */
-	Quaternion(const Vector &v);
+	Quaternion(const float d[4]) : Vector<4>(d) {}
 
 	/**
-	 * ctor from EulerAngles
+	 * setting ctor
 	 */
-	Quaternion(const EulerAngles &euler);
+	Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {}
+
+	using Vector<4>::operator *;
 
 	/**
-	 * ctor from Dcm
+	 * multiplication
 	 */
-	Quaternion(const Dcm &dcm);
+	const Quaternion operator *(const Quaternion &q) const {
+		return Quaternion(
+			       data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
+			       data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
+			       data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
+			       data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
+	}
 
 	/**
-	 * deep copy ctor
+	 * derivative
 	 */
-	Quaternion(const Quaternion &right);
+	const Quaternion derivative(const Vector<3> &w) {
+		float dataQ[] = {
+			data[0], -data[1], -data[2], -data[3],
+			data[1],  data[0], -data[3],  data[2],
+			data[2],  data[3],  data[0], -data[1],
+			data[3], -data[2],  data[1],  data[0]
+		};
+		Matrix<4, 4> Q(dataQ);
+		Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
+		return Q * v * 0.5f;
+	}
 
 	/**
-	 * dtor
+	 * imaginary part of quaternion
 	 */
-	virtual ~Quaternion();
+	Vector<3> imag(void) {
+		return Vector<3>(&data[1]);
+	}
 
 	/**
-	 * derivative
+	 * set quaternion to rotation defined by euler angles
 	 */
-	Vector derivative(const Vector &w);
+	void from_euler(float roll, float pitch, float yaw) {
+		double cosPhi_2 = cos(double(roll) / 2.0);
+		double sinPhi_2 = sin(double(roll) / 2.0);
+		double cosTheta_2 = cos(double(pitch) / 2.0);
+		double sinTheta_2 = sin(double(pitch) / 2.0);
+		double cosPsi_2 = cos(double(yaw) / 2.0);
+		double sinPsi_2 = sin(double(yaw) / 2.0);
+		data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2;
+		data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2;
+		data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
+		data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
+	}
+
+	void from_dcm(const Matrix<3, 3> &m) {
+		// avoiding singularities by not using division equations
+		data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]);
+		data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]);
+		data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]);
+		data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]);
+	}
 
 	/**
-	 * accessors
+	 * create rotation matrix for the quaternion
 	 */
-	void setA(float a) { (*this)(0) = a; }
-	void setB(float b) { (*this)(1) = b; }
-	void setC(float c) { (*this)(2) = c; }
-	void setD(float d) { (*this)(3) = d; }
-	const float &getA() const { return (*this)(0); }
-	const float &getB() const { return (*this)(1); }
-	const float &getC() const { return (*this)(2); }
-	const float &getD() const { return (*this)(3); }
+	Matrix<3, 3> to_dcm(void) const {
+		Matrix<3, 3> R;
+		float aSq = data[0] * data[0];
+		float bSq = data[1] * data[1];
+		float cSq = data[2] * data[2];
+		float dSq = data[3] * data[3];
+		R.data[0][0] = aSq + bSq - cSq - dSq;
+		R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
+		R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
+		R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
+		R.data[1][1] = aSq - bSq + cSq - dSq;
+		R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
+		R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
+		R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
+		R.data[2][2] = aSq - bSq - cSq + dSq;
+		return R;
+	}
 };
 
-int __EXPORT quaternionTest();
-} // math
+}
 
+#endif // QUATERNION_HPP
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 73de793d5..c7323c215 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -1,6 +1,9 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *           Pavel Kirienko <pavel.kirienko@gmail.com>
+ *           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
@@ -32,26 +35,466 @@
  ****************************************************************************/
 
 /**
- * @file Vector.h
+ * @file Vector.hpp
  *
- * math vector
+ * Vector class
  */
 
-#pragma once
+#ifndef VECTOR_HPP
+#define VECTOR_HPP
 
-#include <nuttx/config.h>
-
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Vector.hpp"
-#else
-#include "generic/Vector.hpp"
-#endif
+#include <stdio.h>
+#include <math.h>
+#include "../CMSIS/Include/arm_math.h"
 
 namespace math
 {
-class Vector;
-int __EXPORT vectorTest();
-int __EXPORT vectorAddTest();
-int __EXPORT vectorSubTest();
-bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
-} // math
+
+template <unsigned int N>
+class __EXPORT Vector;
+
+template <unsigned int N>
+class __EXPORT VectorBase
+{
+public:
+	/**
+	 * vector data
+	 */
+	float data[N];
+
+	/**
+	 * struct for using arm_math functions, represents column vector
+	 */
+	arm_matrix_instance_f32 arm_col;
+
+	/**
+	 * trivial ctor
+	 * note that this ctor will not initialize elements
+	 */
+	VectorBase() {
+		arm_col = {N, 1, &data[0]};
+	}
+
+	/**
+	 * copy ctor
+	 */
+	VectorBase(const VectorBase<N> &v) {
+		arm_col = {N, 1, &data[0]};
+		memcpy(data, v.data, sizeof(data));
+	}
+
+	/**
+	 * setting ctor
+	 */
+	VectorBase(const float d[N]) {
+		arm_col = {N, 1, &data[0]};
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[N]) {
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * access to elements by index
+	 */
+	float &operator()(const unsigned int i) {
+		return data[i];
+	}
+
+	/**
+	 * access to elements by index
+	 */
+	float operator()(const unsigned int i) const {
+		return data[i];
+	}
+
+	/**
+	 * get vector size
+	 */
+	unsigned int get_size() const {
+		return N;
+	}
+
+	/**
+	 * test for equality
+	 */
+	bool operator ==(const Vector<N> &v) const {
+		for (unsigned int i = 0; i < N; i++)
+			if (data[i] != v.data[i])
+				return false;
+
+		return true;
+	}
+
+	/**
+	 * test for inequality
+	 */
+	bool operator !=(const Vector<N> &v) const {
+		for (unsigned int i = 0; i < N; i++)
+			if (data[i] != v.data[i])
+				return true;
+
+		return false;
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<N> &operator =(const Vector<N> &v) {
+		memcpy(data, v.data, sizeof(data));
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * negation
+	 */
+	const Vector<N> operator -(void) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = -data[i];
+
+		return res;
+	}
+
+	/**
+	 * addition
+	 */
+	const Vector<N> operator +(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] + v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * subtraction
+	 */
+	const Vector<N> operator -(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] - v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	const Vector<N> operator *(const float num) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] * num;
+
+		return res;
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	const Vector<N> operator /(const float num) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] / num;
+
+		return res;
+	}
+
+	/**
+	 * addition
+	 */
+	const Vector<N> &operator +=(const Vector<N> &v) {
+		for (unsigned int i = 0; i < N; i++)
+			data[i] += v.data[i];
+
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * subtraction
+	 */
+	const Vector<N> &operator -=(const Vector<N> &v) {
+		for (unsigned int i = 0; i < N; i++)
+			data[i] -= v.data[i];
+
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	const Vector<N> &operator *=(const float num) {
+		for (unsigned int i = 0; i < N; i++)
+			data[i] *= num;
+
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	const Vector<N> &operator /=(const float num) {
+		for (unsigned int i = 0; i < N; i++)
+			data[i] /= num;
+
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * dot product
+	 */
+	float operator *(const Vector<N> &v) const {
+		float res = 0.0f;
+
+		for (unsigned int i = 0; i < N; i++)
+			res += data[i] * v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * element by element multiplication
+	 */
+	const Vector<N> emult(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] * v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * element by element division
+	 */
+	const Vector<N> edivide(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] / v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * gets the length of this vector squared
+	 */
+	float length_squared() const {
+		float res = 0.0f;
+
+		for (unsigned int i = 0; i < N; i++)
+			res += data[i] * data[i];
+
+		return res;
+	}
+
+	/**
+	 * gets the length of this vector
+	 */
+	float length() const {
+		float res = 0.0f;
+
+		for (unsigned int i = 0; i < N; i++)
+			res += data[i] * data[i];
+
+		return sqrtf(res);
+	}
+
+	/**
+	 * normalizes this vector
+	 */
+	void normalize() {
+		*this /= length();
+	}
+
+	/**
+	 * returns the normalized version of this vector
+	 */
+	Vector<N> normalized() const {
+		return *this / length();
+	}
+
+	/**
+	 * set zero vector
+	 */
+	void zero(void) {
+		memset(data, 0, sizeof(data));
+	}
+
+	void print(void) {
+		printf("[ ");
+
+		for (unsigned int i = 0; i < N; i++)
+			printf("%.3f\t", data[i]);
+
+		printf("]\n");
+	}
+};
+
+template <unsigned int N>
+class __EXPORT Vector : public VectorBase<N>
+{
+public:
+	Vector() : VectorBase<N>() {}
+
+	Vector(const Vector<N> &v) : VectorBase<N>(v) {}
+
+	Vector(const float d[N]) : VectorBase<N>(d) {}
+
+	/**
+	 * set to value
+	 */
+	const Vector<N> &operator =(const Vector<N> &v) {
+		memcpy(this->data, v.data, sizeof(this->data));
+		return *this;
+	}
+};
+
+template <>
+class __EXPORT Vector<2> : public VectorBase<2>
+{
+public:
+	Vector() : VectorBase<2>() {}
+
+	// simple copy is 1.6 times faster than memcpy
+	Vector(const Vector<2> &v) : VectorBase<2>() {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+	}
+
+	Vector(const float d[2]) : VectorBase<2>() {
+		data[0] = d[0];
+		data[1] = d[1];
+	}
+
+	Vector(const float x, const float y) : VectorBase<2>() {
+		data[0] = x;
+		data[1] = y;
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[2]) {
+		data[0] = d[0];
+		data[1] = d[1];
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<2> &operator =(const Vector<2> &v) {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+		return *this;
+	}
+
+	float operator %(const Vector<2> &v) const {
+		return data[0] * v.data[1] - data[1] * v.data[0];
+	}
+};
+
+template <>
+class __EXPORT Vector<3> : public VectorBase<3>
+{
+public:
+	Vector() : VectorBase<3>() {}
+
+	// simple copy is 1.6 times faster than memcpy
+	Vector(const Vector<3> &v) : VectorBase<3>() {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = v.data[i];
+	}
+
+	Vector(const float d[3]) : VectorBase<3>() {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = d[i];
+	}
+
+	Vector(const float x, const float y, const float z) : VectorBase<3>() {
+		data[0] = x;
+		data[1] = y;
+		data[2] = z;
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[3]) {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = d[i];
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<3> &operator =(const Vector<3> &v) {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = v.data[i];
+
+		return *this;
+	}
+
+	Vector<3> operator %(const Vector<3> &v) const {
+		return Vector<3>(
+			       data[1] * v.data[2] - data[2] * v.data[1],
+			       data[2] * v.data[0] - data[0] * v.data[2],
+			       data[0] * v.data[1] - data[1] * v.data[0]
+		       );
+	}
+};
+
+template <>
+class __EXPORT Vector<4> : public VectorBase<4>
+{
+public:
+	Vector() : VectorBase() {}
+
+	Vector(const Vector<4> &v) : VectorBase<4>() {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = v.data[i];
+	}
+
+	Vector(const float d[4]) : VectorBase<4>() {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = d[i];
+	}
+
+	Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() {
+		data[0] = x0;
+		data[1] = x1;
+		data[2] = x2;
+		data[3] = x3;
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[4]) {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = d[i];
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<4> &operator =(const Vector<4> &v) {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = v.data[i];
+
+		return *this;
+	}
+};
+
+}
+
+#endif // VECTOR_HPP
diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
deleted file mode 100644
index 1945bb02d..000000000
--- a/src/lib/mathlib/math/arm/Matrix.hpp
+++ /dev/null
@@ -1,292 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 Matrix.h
- *
- * matrix code
- */
-
-#pragma once
-
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../Matrix.hpp"
-
-// arm specific
-#include "../../CMSIS/Include/arm_math.h"
-
-namespace math
-{
-
-class __EXPORT Matrix
-{
-public:
-	// constructor
-	Matrix(size_t rows, size_t cols) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 rows, cols,
-				 (float *)calloc(rows * cols, sizeof(float)));
-	}
-	Matrix(size_t rows, size_t cols, const float *data) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 rows, cols,
-				 (float *)malloc(rows * cols * sizeof(float)));
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Matrix() {
-		delete [] _matrix.pData;
-	}
-	// copy constructor (deep)
-	Matrix(const Matrix &right) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 right.getRows(), right.getCols(),
-				 (float *)malloc(right.getRows()*
-						 right.getCols()*sizeof(float)));
-		memcpy(getData(), right.getData(),
-		       getSize());
-	}
-	// assignment
-	inline Matrix &operator=(const Matrix &right) {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i, size_t j) {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	inline const float &operator()(size_t i, size_t j) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				float sig;
-				int exponent;
-				float num = (*this)(i, j);
-				float2SigExp(num, sig, exponent);
-				printf("%6.3fe%03d ", (double)sig, exponent);
-			}
-
-			printf("\n");
-		}
-	}
-	// boolean ops
-	inline bool operator==(const Matrix &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
-					return false;
-			}
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Matrix operator+(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_offset_f32((float *)getData(), right,
-			       (float *)result.getData(), getRows()*getCols());
-		return result;
-	}
-	inline Matrix operator-(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_offset_f32((float *)getData(), -right,
-			       (float *)result.getData(), getRows()*getCols());
-		return result;
-	}
-	inline Matrix operator*(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_mat_scale_f32(&_matrix, right,
-				  &(result._matrix));
-		return result;
-	}
-	inline Matrix operator/(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_mat_scale_f32(&_matrix, 1.0f / right,
-				  &(result._matrix));
-		return result;
-	}
-	// vector ops
-	inline Vector operator*(const Vector &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix resultMat = (*this) *
-				   Matrix(right.getRows(), 1, right.getData());
-		return Vector(getRows(), resultMat.getData());
-	}
-	// matrix ops
-	inline Matrix operator+(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		arm_mat_add_f32(&_matrix, &(right._matrix),
-				&(result._matrix));
-		return result;
-	}
-	inline Matrix operator-(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		arm_mat_sub_f32(&_matrix, &(right._matrix),
-				&(result._matrix));
-		return result;
-	}
-	inline Matrix operator*(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix result(getRows(), right.getCols());
-		arm_mat_mult_f32(&_matrix, &(right._matrix),
-				 &(result._matrix));
-		return result;
-	}
-	inline Matrix operator/(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(right.getRows() == right.getCols());
-		ASSERT(getCols() == right.getCols());
-#endif
-		return (*this) * right.inverse();
-	}
-	// other functions
-	inline Matrix transpose() const {
-		Matrix result(getCols(), getRows());
-		arm_mat_trans_f32(&_matrix, &(result._matrix));
-		return result;
-	}
-	inline void swapRows(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t j = 0; j < getCols(); j++) {
-			float tmp = (*this)(a, j);
-			(*this)(a, j) = (*this)(b, j);
-			(*this)(b, j) = tmp;
-		}
-	}
-	inline void swapCols(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			float tmp = (*this)(i, a);
-			(*this)(i, a) = (*this)(i, b);
-			(*this)(i, b) = tmp;
-		}
-	}
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix inverse() const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		Matrix work = (*this);
-		arm_mat_inverse_f32(&(work._matrix),
-				    &(result._matrix));
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				(*this)(i, j) = val;
-			}
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _matrix.numRows; }
-	inline size_t getCols() const { return _matrix.numCols; }
-	inline static Matrix identity(size_t size) {
-		Matrix result(size, size);
-
-		for (size_t i = 0; i < size; i++) {
-			result(i, i) = 1.0f;
-		}
-
-		return result;
-	}
-	inline static Matrix zero(size_t size) {
-		Matrix result(size, size);
-		result.setAll(0.0f);
-		return result;
-	}
-	inline static Matrix zero(size_t m, size_t n) {
-		Matrix result(m, n);
-		result.setAll(0.0f);
-		return result;
-	}
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
-	inline float *getData() { return _matrix.pData; }
-	inline const float *getData() const { return _matrix.pData; }
-	inline void setData(float *data) { _matrix.pData = data; }
-private:
-	arm_matrix_instance_f32 _matrix;
-};
-
-} // namespace math
diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
deleted file mode 100644
index 52220fc15..000000000
--- a/src/lib/mathlib/math/arm/Vector.hpp
+++ /dev/null
@@ -1,236 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../test/test.hpp"
-
-// arm specific
-#include "../../CMSIS/Include/arm_math.h"
-
-namespace math
-{
-
-class __EXPORT Vector
-{
-public:
-	// constructor
-	Vector(size_t rows) :
-		_rows(rows),
-		_data((float *)calloc(rows, sizeof(float))) {
-	}
-	Vector(size_t rows, const float *data) :
-		_rows(rows),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Vector() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Vector(const Vector &right) :
-		_rows(right.getRows()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Vector &operator=(const Vector &right) {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i) {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	inline const float &operator()(size_t i) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			float sig;
-			int exponent;
-			float num = (*this)(i);
-			float2SigExp(num, sig, exponent);
-			printf("%6.3fe%03d ", (double)sig, exponent);
-		}
-
-		printf("\n");
-	}
-	// boolean ops
-	inline bool operator==(const Vector &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
-				return false;
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Vector operator+(float right) const {
-		Vector result(getRows());
-		arm_offset_f32((float *)getData(),
-			       right, result.getData(),
-			       getRows());
-		return result;
-	}
-	inline Vector operator-(float right) const {
-		Vector result(getRows());
-		arm_offset_f32((float *)getData(),
-			       -right, result.getData(),
-			       getRows());
-		return result;
-	}
-	inline Vector operator*(float right) const {
-		Vector result(getRows());
-		arm_scale_f32((float *)getData(),
-			      right, result.getData(),
-			      getRows());
-		return result;
-	}
-	inline Vector operator/(float right) const {
-		Vector result(getRows());
-		arm_scale_f32((float *)getData(),
-			      1.0f / right, result.getData(),
-			      getRows());
-		return result;
-	}
-	// vector ops
-	inline Vector operator+(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-		arm_add_f32((float *)getData(),
-			    (float *)right.getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	inline Vector operator-(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-		arm_sub_f32((float *)getData(),
-			    (float *)right.getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	inline Vector operator-(void) const {
-		Vector result(getRows());
-		arm_negate_f32((float *)getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	// other functions
-	inline float dot(const Vector &right) const {
-		float result = 0;
-		arm_dot_prod_f32((float *)getData(),
-				 (float *)right.getData(),
-				 getRows(),
-				 &result);
-		return result;
-	}
-	inline float norm() const {
-		return sqrtf(dot(*this));
-	}
-	inline float length() const {
-		return norm();
-	}
-	inline Vector unit() const {
-		return (*this) / norm();
-	}
-	inline Vector normalized() const {
-		return unit();
-	}
-	inline void normalize() {
-		(*this) = (*this) / norm();
-	}
-	inline static Vector zero(size_t rows) {
-		Vector result(rows);
-		// calloc returns zeroed memory
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			(*this)(i) = val;
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-	inline const float *getData() const { return _data; }
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows(); }
-	inline float *getData() { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	float *_data;
-};
-
-} // math
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index 3699d9bce..6f640c9f9 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
     // do the filtering
     float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
     if (isnan(delay_element_0) || isinf(delay_element_0)) {
-        // don't allow bad values to propogate via the filter
+        // don't allow bad values to propagate via the filter
         delay_element_0 = sample;
     }
     float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
     return output;
 }
 
+float LowPassFilter2p::reset(float sample) {
+    _delay_element_1 = _delay_element_2 = sample;
+    return apply(sample);
+}
+
 } // namespace math
 
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 208ec98d4..74cd5d78c 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -52,18 +52,30 @@ public:
         _delay_element_1 = _delay_element_2 = 0;
     }
 
-    // change parameters
+    /**
+     * Change filter parameters
+     */
     void set_cutoff_frequency(float sample_freq, float cutoff_freq);
 
-    // apply - Add a new raw value to the filter 
-    // and retrieve the filtered result
+    /**
+     * Add a new raw value to the filter
+     *
+     * @return retrieve the filtered result
+     */
     float apply(float sample);
 
-    // return the cutoff frequency
+    /**
+     * Return the cutoff frequency
+     */
     float get_cutoff_freq(void) const {
         return _cutoff_freq;
     }
 
+    /**
+     * Reset the filter state to this value
+     */
+    float reset(float sample);
+
 private:
     float           _cutoff_freq; 
     float           _a1;
diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp
deleted file mode 100644
index 5601a3447..000000000
--- a/src/lib/mathlib/math/generic/Matrix.hpp
+++ /dev/null
@@ -1,437 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 Matrix.h
- *
- * matrix code
- */
-
-#pragma once
-
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../Matrix.hpp"
-
-namespace math
-{
-
-class __EXPORT Matrix
-{
-public:
-	// constructor
-	Matrix(size_t rows, size_t cols) :
-		_rows(rows),
-		_cols(cols),
-		_data((float *)calloc(rows *cols, sizeof(float))) {
-	}
-	Matrix(size_t rows, size_t cols, const float *data) :
-		_rows(rows),
-		_cols(cols),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Matrix() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Matrix(const Matrix &right) :
-		_rows(right.getRows()),
-		_cols(right.getCols()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Matrix &operator=(const Matrix &right) {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i, size_t j) {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	inline const float &operator()(size_t i, size_t j) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				float sig;
-				int exp;
-				float num = (*this)(i, j);
-				float2SigExp(num, sig, exp);
-				printf("%6.3fe%03.3d,", (double)sig, exp);
-			}
-
-			printf("\n");
-		}
-	}
-	// boolean ops
-	inline bool operator==(const Matrix &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
-					return false;
-			}
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Matrix operator+(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) + right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator-(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) - right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator*(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) * right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator/(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) / right;
-			}
-		}
-
-		return result;
-	}
-	// vector ops
-	inline Vector operator*(const Vector &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i) += (*this)(i, j) * right(j);
-			}
-		}
-
-		return result;
-	}
-	// matrix ops
-	inline Matrix operator+(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) + right(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator-(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) - right(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator*(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix result(getRows(), right.getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < right.getCols(); j++) {
-				for (size_t k = 0; k < right.getRows(); k++) {
-					result(i, j) += (*this)(i, k) * right(k, j);
-				}
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator/(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(right.getRows() == right.getCols());
-		ASSERT(getCols() == right.getCols());
-#endif
-		return (*this) * right.inverse();
-	}
-	// other functions
-	inline Matrix transpose() const {
-		Matrix result(getCols(), getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(j, i) = (*this)(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline void swapRows(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t j = 0; j < getCols(); j++) {
-			float tmp = (*this)(a, j);
-			(*this)(a, j) = (*this)(b, j);
-			(*this)(b, j) = tmp;
-		}
-	}
-	inline void swapCols(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			float tmp = (*this)(i, a);
-			(*this)(i, a) = (*this)(i, b);
-			(*this)(i, b) = tmp;
-		}
-	}
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix inverse() const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == getCols());
-#endif
-		size_t N = getRows();
-		Matrix L = identity(N);
-		const Matrix &A = (*this);
-		Matrix U = A;
-		Matrix P = identity(N);
-
-		//printf("A:\n"); A.print();
-
-		// for all diagonal elements
-		for (size_t n = 0; n < N; n++) {
-
-			// if diagonal is zero, swap with row below
-			if (fabsf(U(n, n)) < 1e-8f) {
-				//printf("trying pivot for row %d\n",n);
-				for (size_t i = 0; i < N; i++) {
-					if (i == n) continue;
-
-					//printf("\ttrying row %d\n",i);
-					if (fabsf(U(i, n)) > 1e-8f) {
-						//printf("swapped %d\n",i);
-						U.swapRows(i, n);
-						P.swapRows(i, n);
-					}
-				}
-			}
-
-#ifdef MATRIX_ASSERT
-			//printf("A:\n"); A.print();
-			//printf("U:\n"); U.print();
-			//printf("P:\n"); P.print();
-			//fflush(stdout);
-			ASSERT(fabsf(U(n, n)) > 1e-8f);
-#endif
-
-			// failsafe, return zero matrix
-			if (fabsf(U(n, n)) < 1e-8f) {
-				return Matrix::zero(n);
-			}
-
-			// for all rows below diagonal
-			for (size_t i = (n + 1); i < N; i++) {
-				L(i, n) = U(i, n) / U(n, n);
-
-				// add i-th row and n-th row
-				// multiplied by: -a(i,n)/a(n,n)
-				for (size_t k = n; k < N; k++) {
-					U(i, k) -= L(i, n) * U(n, k);
-				}
-			}
-		}
-
-		//printf("L:\n"); L.print();
-		//printf("U:\n"); U.print();
-
-		// solve LY=P*I for Y by forward subst
-		Matrix Y = P;
-
-		// for all columns of Y
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of L
-			for (size_t i = 0; i < N; i++) {
-				// for all columns of L
-				for (size_t j = 0; j < i; j++) {
-					// for all existing y
-					// subtract the component they
-					// contribute to the solution
-					Y(i, c) -= L(i, j) * Y(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				// Y(i,c) /= L(i,i);
-				// but L(i,i) = 1.0
-			}
-		}
-
-		//printf("Y:\n"); Y.print();
-
-		// solve Ux=y for x by back subst
-		Matrix X = Y;
-
-		// for all columns of X
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of U
-			for (size_t k = 0; k < N; k++) {
-				// have to go in reverse order
-				size_t i = N - 1 - k;
-
-				// for all columns of U
-				for (size_t j = i + 1; j < N; j++) {
-					// for all existing x
-					// subtract the component they
-					// contribute to the solution
-					X(i, c) -= U(i, j) * X(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				X(i, c) /= U(i, i);
-			}
-		}
-
-		//printf("X:\n"); X.print();
-		return X;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				(*this)(i, j) = val;
-			}
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-	inline size_t getCols() const { return _cols; }
-	inline static Matrix identity(size_t size) {
-		Matrix result(size, size);
-
-		for (size_t i = 0; i < size; i++) {
-			result(i, i) = 1.0f;
-		}
-
-		return result;
-	}
-	inline static Matrix zero(size_t size) {
-		Matrix result(size, size);
-		result.setAll(0.0f);
-		return result;
-	}
-	inline static Matrix zero(size_t m, size_t n) {
-		Matrix result(m, n);
-		result.setAll(0.0f);
-		return result;
-	}
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
-	inline float *getData() { return _data; }
-	inline const float *getData() const { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	size_t _cols;
-	float *_data;
-};
-
-} // namespace math
diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp
deleted file mode 100644
index 8cfdc676d..000000000
--- a/src/lib/mathlib/math/generic/Vector.hpp
+++ /dev/null
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-
-namespace math
-{
-
-class __EXPORT Vector
-{
-public:
-	// constructor
-	Vector(size_t rows) :
-		_rows(rows),
-		_data((float *)calloc(rows, sizeof(float))) {
-	}
-	Vector(size_t rows, const float *data) :
-		_rows(rows),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Vector() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Vector(const Vector &right) :
-		_rows(right.getRows()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Vector &operator=(const Vector &right) {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i) {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	inline const float &operator()(size_t i) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			float sig;
-			int exp;
-			float num = (*this)(i);
-			float2SigExp(num, sig, exp);
-			printf("%6.3fe%03.3d,", (double)sig, exp);
-		}
-
-		printf("\n");
-	}
-	// boolean ops
-	inline bool operator==(const Vector &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
-				return false;
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Vector operator+(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) + right;
-		}
-
-		return result;
-	}
-	inline Vector operator-(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) - right;
-		}
-
-		return result;
-	}
-	inline Vector operator*(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) * right;
-		}
-
-		return result;
-	}
-	inline Vector operator/(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) / right;
-		}
-
-		return result;
-	}
-	// vector ops
-	inline Vector operator+(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) + right(i);
-		}
-
-		return result;
-	}
-	inline Vector operator-(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) - right(i);
-		}
-
-		return result;
-	}
-	inline Vector operator-(void) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = -((*this)(i));
-		}
-
-		return result;
-	}
-	// other functions
-	inline float dot(const Vector &right) const {
-		float result = 0;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result += (*this)(i) * (*this)(i);
-		}
-
-		return result;
-	}
-	inline float norm() const {
-		return sqrtf(dot(*this));
-	}
-	inline float length() const {
-		return norm();
-	}
-	inline Vector unit() const {
-		return (*this) / norm();
-	}
-	inline Vector normalized() const {
-		return unit();
-	}
-	inline void normalize() {
-		(*this) = (*this) / norm();
-	}
-	inline static Vector zero(size_t rows) {
-		Vector result(rows);
-		// calloc returns zeroed memory
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			(*this)(i) = val;
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows(); }
-	inline float *getData() { return _data; }
-	inline const float *getData() const { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	float *_data;
-};
-
-} // math
diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h
index 40ffb22bc..9e03855c5 100644
--- a/src/lib/mathlib/mathlib.h
+++ b/src/lib/mathlib/mathlib.h
@@ -41,13 +41,9 @@
 
 #pragma once
 
-#include "math/Dcm.hpp"
-#include "math/EulerAngles.hpp"
+#include "math/Vector.hpp"
 #include "math/Matrix.hpp"
 #include "math/Quaternion.hpp"
-#include "math/Vector.hpp"
-#include "math/Vector3.hpp"
-#include "math/Vector2f.hpp"
 #include "math/Limits.hpp"
 
 #endif
@@ -56,4 +52,4 @@
 
 #include "CMSIS/Include/arm_math.h"
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk
index 72bc7db8a..191e2da73 100644
--- a/src/lib/mathlib/module.mk
+++ b/src/lib/mathlib/module.mk
@@ -35,13 +35,6 @@
 # Math library
 #
 SRCS		 = math/test/test.cpp \
-		   math/Vector.cpp \
-		   math/Vector2f.cpp \
-		   math/Vector3.cpp \
-		   math/EulerAngles.cpp \
-		   math/Quaternion.cpp \
-		   math/Dcm.cpp \
-		   math/Matrix.cpp \
 		   math/Limits.cpp
 
 #
@@ -49,13 +42,3 @@ SRCS		 = math/test/test.cpp \
 # current makefile name, since app.mk needs it.
 #
 APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
-
-ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
-INCLUDE_DIRS	+= math/arm
-SRCS		+= math/arm/Vector.cpp \
-		   math/arm/Matrix.cpp
-else
-#INCLUDE_DIRS	+= math/generic
-#SRCS		+= math/generic/Vector.cpp \
-#		   math/generic/Matrix.cpp
-endif
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index af733aaf0..d8ccb6774 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -59,4 +59,8 @@
 #define	HW_ARCH "PX4FMU_V2"
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define	HW_ARCH "AEROCORE"
+#endif
+
 #endif /* VERSION_H_ */
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
deleted file mode 100644
index ecca04dd7..000000000
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ /dev/null
@@ -1,825 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 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 KalmanNav.cpp
- *
- * Kalman filter navigation code
- */
-
-#include <poll.h>
-
-#include "KalmanNav.hpp"
-#include <systemlib/err.h>
-#include <geo/geo.h>
-
-// constants
-// Titterton pg. 52
-static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
-static const float R0 = 6378137.0f; // earth radius, m
-static const float g0 = 9.806f; // standard gravitational accel. m/s^2
-static const int8_t ret_ok = 0; 		// no error in function
-static const int8_t ret_error = -1; 	// error occurred
-
-KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	// ekf matrices
-	F(9, 9),
-	G(9, 6),
-	P(9, 9),
-	P0(9, 9),
-	V(6, 6),
-	// attitude measurement ekf matrices
-	HAtt(4, 9),
-	RAtt(4, 4),
-	// position measurement ekf matrices
-	HPos(6, 9),
-	RPos(6, 6),
-	// attitude representations
-	C_nb(),
-	q(),
-	// subscriptions
-	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
-	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
-	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
-	// publications
-	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
-	_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
-	_att(&getPublications(), ORB_ID(vehicle_attitude)),
-	// timestamps
-	_pubTimeStamp(hrt_absolute_time()),
-	_predictTimeStamp(hrt_absolute_time()),
-	_attTimeStamp(hrt_absolute_time()),
-	_outTimeStamp(hrt_absolute_time()),
-	// frame count
-	_navFrames(0),
-	// miss counts
-	_miss(0),
-	// accelerations
-	fN(0), fE(0), fD(0),
-	// state
-	phi(0), theta(0), psi(0),
-	vN(0), vE(0), vD(0),
-	lat(0), lon(0), alt(0),
-	lat0(0), lon0(0), alt0(0),
-	// parameters for ground station
-	_vGyro(this, "V_GYRO"),
-	_vAccel(this, "V_ACCEL"),
-	_rMag(this, "R_MAG"),
-	_rGpsVel(this, "R_GPS_VEL"),
-	_rGpsPos(this, "R_GPS_POS"),
-	_rGpsAlt(this, "R_GPS_ALT"),
-	_rPressAlt(this, "R_PRESS_ALT"),
-	_rAccel(this, "R_ACCEL"),
-	_magDip(this, "ENV_MAG_DIP"),
-	_magDec(this, "ENV_MAG_DEC"),
-	_g(this, "ENV_G"),
-	_faultPos(this, "FAULT_POS"),
-	_faultAtt(this, "FAULT_ATT"),
-	_attitudeInitialized(false),
-	_positionInitialized(false),
-	_attitudeInitCounter(0)
-{
-	using namespace math;
-
-	// initial state covariance matrix
-	P0 = Matrix::identity(9) * 0.01f;
-	P = P0;
-
-	// initial state
-	phi = 0.0f;
-	theta = 0.0f;
-	psi = 0.0f;
-	vN = 0.0f;
-	vE = 0.0f;
-	vD = 0.0f;
-	lat = 0.0f;
-	lon = 0.0f;
-	alt = 0.0f;
-
-	// initialize rotation quaternion with a single raw sensor measurement
-	_sensors.update();
-	q = init(
-		_sensors.accelerometer_m_s2[0],
-		_sensors.accelerometer_m_s2[1],
-		_sensors.accelerometer_m_s2[2],
-		_sensors.magnetometer_ga[0],
-		_sensors.magnetometer_ga[1],
-		_sensors.magnetometer_ga[2]);
-
-	// initialize dcm
-	C_nb = Dcm(q);
-
-	// HPos is constant
-	HPos(0, 3) = 1.0f;
-	HPos(1, 4) = 1.0f;
-	HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
-	HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
-	HPos(4, 8) = 1.0f;
-	HPos(5, 8) = 1.0f;
-
-	// initialize all parameters
-	updateParams();
-}
-
-math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
-{
-    float initialRoll, initialPitch;
-    float cosRoll, sinRoll, cosPitch, sinPitch;
-    float magX, magY;
-    float initialHdg, cosHeading, sinHeading;
-
-    initialRoll = atan2(-ay, -az);
-    initialPitch = atan2(ax, -az);
-
-    cosRoll = cosf(initialRoll);
-    sinRoll = sinf(initialRoll);
-    cosPitch = cosf(initialPitch);
-    sinPitch = sinf(initialPitch);
-
-    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
-
-    magY = my * cosRoll - mz * sinRoll;
-
-    initialHdg = atan2f(-magY, magX);
-
-    cosRoll = cosf(initialRoll * 0.5f);
-    sinRoll = sinf(initialRoll * 0.5f);
-
-    cosPitch = cosf(initialPitch * 0.5f);
-    sinPitch = sinf(initialPitch * 0.5f);
-
-    cosHeading = cosf(initialHdg * 0.5f);
-    sinHeading = sinf(initialHdg * 0.5f);
-
-    float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
-    float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
-    float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
-    float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
-
-    return math::Quaternion(q0, q1, q2, q3);
-
-}
-
-void KalmanNav::update()
-{
-	using namespace math;
-
-	struct pollfd fds[1];
-	fds[0].fd = _sensors.getHandle();
-	fds[0].events = POLLIN;
-
-	// poll for new data
-	int ret = poll(fds, 1, 1000);
-
-	if (ret < 0) {
-		// XXX this is seriously bad - should be an emergency
-		return;
-
-	} else if (ret == 0) { // timeout
-		return;
-	}
-
-	// get new timestamp
-	uint64_t newTimeStamp = hrt_absolute_time();
-
-	// check updated subscriptions
-	if (_param_update.updated()) updateParams();
-
-	bool gpsUpdate = _gps.updated();
-	bool sensorsUpdate = _sensors.updated();
-
-	// get new information from subscriptions
-	// this clears update flag
-	updateSubscriptions();
-
-	// initialize attitude when sensors online
-	if (!_attitudeInitialized && sensorsUpdate) {
-		if (correctAtt() == ret_ok) _attitudeInitCounter++;
-
-		if (_attitudeInitCounter > 100) {
-			warnx("initialized EKF attitude\n");
-			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
-			       double(phi), double(theta), double(psi));
-			_attitudeInitialized = true;
-		}
-	}
-
-	// initialize position when gps received
-	if (!_positionInitialized &&
-	    _attitudeInitialized && // wait for attitude first
-	    gpsUpdate &&
-	    _gps.fix_type > 2
-	    //&& _gps.counter_pos_valid > 10
-	   ) {
-		vN = _gps.vel_n_m_s;
-		vE = _gps.vel_e_m_s;
-		vD = _gps.vel_d_m_s;
-		setLatDegE7(_gps.lat);
-		setLonDegE7(_gps.lon);
-		setAltE3(_gps.alt);
-		// set reference position for
-		// local position
-		lat0 = lat;
-		lon0 = lon;
-		alt0 = alt;
-		// XXX map_projection has internal global
-		// states that multiple things could change, 
-		// should make map_projection take reference
-		// lat/lon and not have init
-		map_projection_init(lat0, lon0);
-		_positionInitialized = true;
-		warnx("initialized EKF state with GPS\n");
-		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
-		       double(vN), double(vE), double(vD),
-		       lat, lon, double(alt));
-	}
-
-	// prediction step
-	// using sensors timestamp so we can account for packet lag
-	float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
-	//printf("dt: %15.10f\n", double(dt));
-	_predictTimeStamp = _sensors.timestamp;
-
-	// don't predict if time greater than a second
-	if (dt < 1.0f) {
-		predictState(dt);
-		predictStateCovariance(dt);
-		// count fast frames
-		_navFrames += 1;
-	}
-
-	// count times 100 Hz rate isn't met
-	if (dt > 0.01f) _miss++;
-
-	// gps correction step
-	if (_positionInitialized && gpsUpdate) {
-		correctPos();
-	}
-
-	// attitude correction step
-	if (_attitudeInitialized 								// initialized
-	    && sensorsUpdate 								// new data
-	    && _sensors.timestamp - _attTimeStamp > 1e6 / 50 	// 50 Hz
-	   ) {
-		_attTimeStamp = _sensors.timestamp;
-		correctAtt();
-	}
-
-	// publication
-	if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
-		_pubTimeStamp = newTimeStamp;
-
-		updatePublications();
-	}
-
-	// output
-	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
-		_outTimeStamp = newTimeStamp;
-		//printf("nav: %4d Hz, miss #: %4d\n",
-		//       _navFrames / 10, _miss / 10);
-		_navFrames = 0;
-		_miss = 0;
-	}
-}
-
-void KalmanNav::updatePublications()
-{
-	using namespace math;
-
-	// global position publication
-	_pos.timestamp = _pubTimeStamp;
-	_pos.time_gps_usec = _gps.timestamp_position;
-	_pos.valid = true;
-	_pos.lat = getLatDegE7();
-	_pos.lon = getLonDegE7();
-	_pos.alt = float(alt);
-	_pos.relative_alt = float(alt); // TODO, make relative
-	_pos.vx = vN;
-	_pos.vy = vE;
-	_pos.vz = vD;
-	_pos.yaw = psi;
-
-	// local position publication
-	float x;
-	float y;
-	bool landed = alt < (alt0 + 0.1); // XXX improve?
-	map_projection_project(lat, lon, &x, &y); 
-	_localPos.timestamp = _pubTimeStamp;
-	_localPos.xy_valid = true;
-	_localPos.z_valid = true;
-	_localPos.v_xy_valid = true;
-	_localPos.v_z_valid = true;
-	_localPos.x = x;
-	_localPos.y = y;
-	_localPos.z = alt0 - alt;
-	_localPos.vx = vN;
-	_localPos.vy = vE;
-	_localPos.vz = vD;
-	_localPos.yaw = psi;
-	_localPos.xy_global = true;
-	_localPos.z_global = true;
-	_localPos.ref_timestamp = _pubTimeStamp;
-	_localPos.ref_lat = getLatDegE7();
-	_localPos.ref_lon = getLonDegE7();
-	_localPos.ref_alt = 0;
-	_localPos.landed = landed;
-
-	// attitude publication
-	_att.timestamp = _pubTimeStamp;
-	_att.roll = phi;
-	_att.pitch = theta;
-	_att.yaw = psi;
-	_att.rollspeed = _sensors.gyro_rad_s[0];
-	_att.pitchspeed = _sensors.gyro_rad_s[1];
-	_att.yawspeed = _sensors.gyro_rad_s[2];
-	// TODO, add gyro offsets to filter
-	_att.rate_offsets[0] = 0.0f;
-	_att.rate_offsets[1] = 0.0f;
-	_att.rate_offsets[2] = 0.0f;
-
-	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-			_att.R[i][j] = C_nb(i, j);
-
-	for (int i = 0; i < 4; i++) _att.q[i] = q(i);
-
-	_att.R_valid = true;
-	_att.q_valid = true;
-
-	// selectively update publications,
-	// do NOT call superblock do-all method
-	if (_positionInitialized) {
-		_pos.update();
-		_localPos.update();
-	}
-
-	if (_attitudeInitialized)
-		_att.update();
-}
-
-int KalmanNav::predictState(float dt)
-{
-	using namespace math;
-
-	// trig
-	float sinL = sinf(lat);
-	float cosL = cosf(lat);
-	float cosLSing = cosf(lat);
-
-	// prevent singularity
-	if (fabsf(cosLSing) < 0.01f) {
-		if (cosLSing > 0) cosLSing = 0.01;
-		else cosLSing = -0.01;
-	}
-
-	// attitude prediction
-	if (_attitudeInitialized) {
-		Vector3 w(_sensors.gyro_rad_s);
-
-		// attitude
-		q = q + q.derivative(w) * dt;
-
-		// renormalize quaternion if needed
-		if (fabsf(q.norm() - 1.0f) > 1e-4f) {
-			q = q.unit();
-		}
-
-		// C_nb update
-		C_nb = Dcm(q);
-
-		// euler update
-		EulerAngles euler(C_nb);
-		phi = euler.getPhi();
-		theta = euler.getTheta();
-		psi = euler.getPsi();
-
-		// specific acceleration in nav frame
-		Vector3 accelB(_sensors.accelerometer_m_s2);
-		Vector3 accelN = C_nb * accelB;
-		fN = accelN(0);
-		fE = accelN(1);
-		fD = accelN(2);
-	}
-
-	// position prediction
-	if (_positionInitialized) {
-		// neglects angular deflections in local gravity
-		// see Titerton pg. 70
-		float R = R0 + float(alt);
-		float LDot = vN / R;
-		float lDot = vE / (cosLSing * R);
-		float rotRate = 2 * omega + lDot;
-
-		// XXX position prediction using speed
-		float vNDot = fN - vE * rotRate * sinL +
-			      vD * LDot;
-		float vDDot = fD - vE * rotRate * cosL -
-			      vN * LDot + _g.get();
-		float vEDot = fE + vN * rotRate * sinL +
-			      vDDot * rotRate * cosL;
-
-		// rectangular integration
-		vN += vNDot * dt;
-		vE += vEDot * dt;
-		vD += vDDot * dt;
-		lat += double(LDot * dt);
-		lon += double(lDot * dt);
-		alt += double(-vD * dt);
-	}
-
-	return ret_ok;
-}
-
-int KalmanNav::predictStateCovariance(float dt)
-{
-	using namespace math;
-
-	// trig
-	float sinL = sinf(lat);
-	float cosL = cosf(lat);
-	float cosLSq = cosL * cosL;
-	float tanL = tanf(lat);
-
-	// prepare for matrix
-	float R = R0 + float(alt);
-	float RSq = R * R;
-
-	// F Matrix
-	// Titterton pg. 291
-
-	F(0, 1) = -(omega * sinL + vE * tanL / R);
-	F(0, 2) = vN / R;
-	F(0, 4) = 1.0f / R;
-	F(0, 6) = -omega * sinL;
-	F(0, 8) = -vE / RSq;
-
-	F(1, 0) = omega * sinL + vE * tanL / R;
-	F(1, 2) = omega * cosL + vE / R;
-	F(1, 3) = -1.0f / R;
-	F(1, 8) = vN / RSq;
-
-	F(2, 0) = -vN / R;
-	F(2, 1) = -omega * cosL - vE / R;
-	F(2, 4) = -tanL / R;
-	F(2, 6) = -omega * cosL - vE / (R * cosLSq);
-	F(2, 8) = vE * tanL / RSq;
-
-	F(3, 1) = -fD;
-	F(3, 2) = fE;
-	F(3, 3) = vD / R;
-	F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
-	F(3, 5) = vN / R;
-	F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
-	F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
-
-	F(4, 0) = fD;
-	F(4, 2) = -fN;
-	F(4, 3) = 2 * omega * sinL + vE * tanL / R;
-	F(4, 4) = (vN * tanL + vD) / R;
-	F(4, 5) = 2 * omega * cosL + vE / R;
-	F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
-		  vN * vE / (R * cosLSq);
-	F(4, 8) = -vE * (vN * tanL + vD) / RSq;
-
-	F(5, 0) = -fE;
-	F(5, 1) = fN;
-	F(5, 3) = -2 * vN / R;
-	F(5, 4) = -2 * (omega * cosL + vE / R);
-	F(5, 6) = 2 * omega * vE * sinL;
-	F(5, 8) = (vN * vN + vE * vE) / RSq;
-
-	F(6, 3) = 1 / R;
-	F(6, 8) = -vN / RSq;
-
-	F(7, 4) = 1 / (R * cosL);
-	F(7, 6) = vE * tanL / (R * cosL);
-	F(7, 8) = -vE / (cosL * RSq);
-
-	F(8, 5) = -1;
-
-	// G Matrix
-	// Titterton pg. 291
-	G(0, 0) = -C_nb(0, 0);
-	G(0, 1) = -C_nb(0, 1);
-	G(0, 2) = -C_nb(0, 2);
-	G(1, 0) = -C_nb(1, 0);
-	G(1, 1) = -C_nb(1, 1);
-	G(1, 2) = -C_nb(1, 2);
-	G(2, 0) = -C_nb(2, 0);
-	G(2, 1) = -C_nb(2, 1);
-	G(2, 2) = -C_nb(2, 2);
-
-	G(3, 3) = C_nb(0, 0);
-	G(3, 4) = C_nb(0, 1);
-	G(3, 5) = C_nb(0, 2);
-	G(4, 3) = C_nb(1, 0);
-	G(4, 4) = C_nb(1, 1);
-	G(4, 5) = C_nb(1, 2);
-	G(5, 3) = C_nb(2, 0);
-	G(5, 4) = C_nb(2, 1);
-	G(5, 5) = C_nb(2, 2);
-
-	// continuous predictioon equations
-	// for discrte time EKF
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
-
-	return ret_ok;
-}
-
-int KalmanNav::correctAtt()
-{
-	using namespace math;
-
-	// trig
-	float cosPhi = cosf(phi);
-	float cosTheta = cosf(theta);
-	// float cosPsi = cosf(psi);
-	float sinPhi = sinf(phi);
-	float sinTheta = sinf(theta);
-	// float sinPsi = sinf(psi);
-
-	// mag predicted measurement
-	// choosing some typical magnetic field properties,
-	//  TODO dip/dec depend on lat/ lon/ time
-	//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
-	float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
-
-	// compensate roll and pitch, but not yaw
-	// XXX take the vectors out of the C_nb matrix to avoid singularities
-	math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
-
-	// mag measurement
-	Vector3 magBody(_sensors.magnetometer_ga);
-
-	// transform to earth frame
-	Vector3 magNav = C_rp * magBody;
-
-	// calculate error between estimate and measurement
-	// apply declination correction for true heading as well.
-	float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
-	if (yMag > M_PI_F) yMag -= 2*M_PI_F;
-	if (yMag < -M_PI_F) yMag += 2*M_PI_F;
-
-	// accel measurement
-	Vector3 zAccel(_sensors.accelerometer_m_s2);
-	float accelMag = zAccel.norm();
-	zAccel = zAccel.unit();
-
-	// ignore accel correction when accel mag not close to g
-	Matrix RAttAdjust = RAtt;
-
-	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
-
-	if (ignoreAccel) {
-		RAttAdjust(1, 1) = 1.0e10;
-		RAttAdjust(2, 2) = 1.0e10;
-		RAttAdjust(3, 3) = 1.0e10;
-
-	} else {
-		//printf("correcting attitude with accel\n");
-	}
-
-	// accel predicted measurement
-	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
-
-	// calculate residual
-	Vector y(4);
-	y(0) = yMag;
-	y(1) = zAccel(0) - zAccelHat(0);
-	y(2) = zAccel(1) - zAccelHat(1);
-	y(3) = zAccel(2) - zAccelHat(2);
-
-	// HMag
-	HAtt(0, 2) = 1;
-
-	// HAccel
-	HAtt(1, 1) = cosTheta;
-	HAtt(2, 0) = -cosPhi * cosTheta;
-	HAtt(2, 1) = sinPhi * sinTheta;
-	HAtt(3, 0) = sinPhi * cosTheta;
-	HAtt(3, 1) = cosPhi * sinTheta;
-
-	// compute correction
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
-	Matrix K = P * HAtt.transpose() * S.inverse();
-	Vector xCorrect = K * y;
-
-	// check correciton is sane
-	for (size_t i = 0; i < xCorrect.getRows(); i++) {
-		float val = xCorrect(i);
-
-		if (isnan(val) || isinf(val)) {
-			// abort correction and return
-			warnx("numerical failure in att correction\n");
-			// reset P matrix to P0
-			P = P0;
-			return ret_error;
-		}
-	}
-
-	// correct state
-	if (!ignoreAccel) {
-		phi += xCorrect(PHI);
-		theta += xCorrect(THETA);
-	}
-
-	psi += xCorrect(PSI);
-
-	// attitude also affects nav velocities
-	if (_positionInitialized) {
-		vN += xCorrect(VN);
-		vE += xCorrect(VE);
-		vD += xCorrect(VD);
-	}
-
-	// update state covariance
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P - K * HAtt * P;
-
-	// fault detection
-	float beta = y.dot(S.inverse() * y);
-
-	if (beta > _faultAtt.get()) {
-		warnx("fault in attitude: beta = %8.4f", (double)beta);
-		warnx("y:"); y.print();
-	}
-
-	// update quaternions from euler
-	// angle correction
-	q = Quaternion(EulerAngles(phi, theta, psi));
-
-	return ret_ok;
-}
-
-int KalmanNav::correctPos()
-{
-	using namespace math;
-
-	// residual
-	Vector y(6);
-	y(0) = _gps.vel_n_m_s - vN;
-	y(1) = _gps.vel_e_m_s - vE;
-	y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
-	y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
-	y(4) = _gps.alt / 1.0e3f - alt;
-	y(5) = _sensors.baro_alt_meter - alt;
-
-	// compute correction
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
-	Matrix K = P * HPos.transpose() * S.inverse();
-	Vector xCorrect = K * y;
-
-	// check correction is sane
-	for (size_t i = 0; i < xCorrect.getRows(); i++) {
-		float val = xCorrect(i);
-
-		if (!isfinite(val)) {
-			// abort correction and return
-			warnx("numerical failure in gps correction\n");
-			// fallback to GPS
-			vN = _gps.vel_n_m_s;
-			vE = _gps.vel_e_m_s;
-			vD = _gps.vel_d_m_s;
-			setLatDegE7(_gps.lat);
-			setLonDegE7(_gps.lon);
-			setAltE3(_gps.alt);
-			// reset P matrix to P0
-			P = P0;
-			return ret_error;
-		}
-	}
-
-	// correct state
-	vN += xCorrect(VN);
-	vE += xCorrect(VE);
-	vD += xCorrect(VD);
-	lat += double(xCorrect(LAT));
-	lon += double(xCorrect(LON));
-	alt += xCorrect(ALT);
-
-	// update state covariance
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P - K * HPos * P;
-
-	// fault detetcion
-	float beta = y.dot(S.inverse() * y);
-
-	static int counter = 0;
-	if (beta > _faultPos.get() && (counter % 10 == 0)) {
-		warnx("fault in gps: beta = %8.4f", (double)beta);
-		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
-		       double(y(0) / sqrtf(RPos(0, 0))),
-		       double(y(1) / sqrtf(RPos(1, 1))),
-		       double(y(2) / sqrtf(RPos(2, 2))),
-		       double(y(3) / sqrtf(RPos(3, 3))),
-		       double(y(4) / sqrtf(RPos(4, 4))),
-		       double(y(5) / sqrtf(RPos(5, 5))));
-	}
-	counter++;
-
-	return ret_ok;
-}
-
-void KalmanNav::updateParams()
-{
-	using namespace math;
-	using namespace control;
-	SuperBlock::updateParams();
-
-	// gyro noise
-	V(0, 0) = _vGyro.get();   // gyro x, rad/s
-	V(1, 1) = _vGyro.get();   // gyro y
-	V(2, 2) = _vGyro.get();   // gyro z
-
-	// accel noise
-	V(3, 3) = _vAccel.get();   // accel x, m/s^2
-	V(4, 4) = _vAccel.get();   // accel y
-	V(5, 5) = _vAccel.get();   // accel z
-
-	// magnetometer noise
-	float noiseMin = 1e-6f;
-	float noiseMagSq = _rMag.get() * _rMag.get();
-
-	if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
-
-	RAtt(0, 0) = noiseMagSq; // normalized direction
-
-	// accelerometer noise
-	float noiseAccelSq = _rAccel.get() * _rAccel.get();
-
-	// bound noise to prevent singularities
-	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
-
-	RAtt(1, 1) = noiseAccelSq; // normalized direction
-	RAtt(2, 2) = noiseAccelSq;
-	RAtt(3, 3) = noiseAccelSq;
-
-	// gps noise
-	float R = R0 + float(alt);
-	float cosLSing = cosf(lat);
-
-	// prevent singularity
-	if (fabsf(cosLSing) < 0.01f) {
-		if (cosLSing > 0) cosLSing = 0.01;
-		else cosLSing = -0.01;
-	}
-
-	float noiseVel = _rGpsVel.get();
-	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
-	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
-	float noiseGpsAlt = _rGpsAlt.get();
-	float noisePressAlt = _rPressAlt.get();
-
-	// bound noise to prevent singularities
-	if (noiseVel < noiseMin) noiseVel = noiseMin;
-
-	if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
-
-	if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
-
-	if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
-
-	if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
-
-	RPos(0, 0) = noiseVel * noiseVel; // vn
-	RPos(1, 1) = noiseVel * noiseVel; // ve
-	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
-	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
-	RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
-	RPos(5, 5) = noisePressAlt * noisePressAlt; // h
-	// XXX, note that RPos depends on lat, so updateParams should
-	// be called if lat changes significantly
-}
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
deleted file mode 100644
index a69bde1a6..000000000
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ /dev/null
@@ -1,192 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 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 KalmanNav.hpp
- *
- * kalman filter navigation code
- */
-
-#pragma once
-
-//#define MATRIX_ASSERT
-//#define VECTOR_ASSERT
-
-#include <nuttx/config.h>
-
-#include <mathlib/mathlib.h>
-#include <controllib/blocks.hpp>
-#include <controllib/block/BlockParam.hpp>
-#include <controllib/uorb/UOrbSubscription.hpp>
-#include <controllib/uorb/UOrbPublication.hpp>
-
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-
-#include <drivers/drv_hrt.h>
-#include <poll.h>
-#include <unistd.h>
-
-/**
- * Kalman filter navigation class
- * http://en.wikipedia.org/wiki/Extended_Kalman_filter
- * Discrete-time extended Kalman filter
- */
-class KalmanNav : public control::SuperBlock
-{
-public:
-	/**
-	 * Constructor
-	 */
-	KalmanNav(SuperBlock *parent, const char *name);
-
-	/**
-	 * Deconstuctor
-	 */
-
-	virtual ~KalmanNav() {};
-
-	math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
-
-	/**
-	 * The main callback function for the class
-	 */
-	void update();
-
-
-	/**
-	 * Publication update
-	 */
-	virtual void updatePublications();
-
-	/**
-	 * State prediction
-	 * Continuous, non-linear
-	 */
-	int predictState(float dt);
-
-	/**
-	 * State covariance prediction
-	 * Continuous, linear
-	 */
-	int predictStateCovariance(float dt);
-
-	/**
-	 * Attitude correction
-	 */
-	int correctAtt();
-
-	/**
-	 * Position correction
-	 */
-	int correctPos();
-
-	/**
-	 * Overloaded update parameters
-	 */
-	virtual void updateParams();
-protected:
-	// kalman filter
-	math::Matrix F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
-	math::Matrix G;             /**< noise shaping matrix for gyro/accel */
-	math::Matrix P;             /**< state covariance matrix */
-	math::Matrix P0;            /**< initial state covariance matrix */
-	math::Matrix V;             /**< gyro/ accel noise matrix */
-	math::Matrix HAtt;          /**< attitude measurement matrix */
-	math::Matrix RAtt;          /**< attitude measurement noise matrix */
-	math::Matrix HPos;          /**< position measurement jacobian matrix */
-	math::Matrix RPos;          /**< position measurement noise matrix */
-	// attitude
-	math::Dcm C_nb;             /**< direction cosine matrix from body to nav frame */
-	math::Quaternion q;         /**< quaternion from body to nav frame */
-	// subscriptions
-	control::UOrbSubscription<sensor_combined_s> _sensors;          /**< sensors sub. */
-	control::UOrbSubscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
-	control::UOrbSubscription<parameter_update_s> _param_update;    /**< parameter update sub. */
-	// publications
-	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
-	control::UOrbPublication<vehicle_local_position_s> _localPos;   /**< local position pub. */
-	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
-	// time stamps
-	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
-	uint64_t _predictTimeStamp; /**< prediction time stamp */
-	uint64_t _attTimeStamp;     /**< attitude correction time stamp */
-	uint64_t _outTimeStamp;     /**< output time stamp */
-	// frame count
-	uint16_t _navFrames;        /**< navigation frames completed in output cycle */
-	// miss counts
-	uint16_t _miss;         	/**< number of times fast prediction loop missed */
-	// accelerations
-	float fN, fE, fD;           /**< navigation frame acceleration */
-	// states
-	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};  /**< state enumeration */
-	float phi, theta, psi;                  /**< 3-2-1 euler angles */
-	float vN, vE, vD;                       /**< navigation velocity, m/s */
-	double lat, lon;                   	/**< lat, lon radians */
-	// parameters
-	float alt;                   		/**< altitude, meters */
-	double lat0, lon0;                   	/**< reference latitude and longitude */
-	float alt0;                   		/**<  refeerence altitude (ground height) */
-	control::BlockParamFloat _vGyro;      /**< gyro process noise */
-	control::BlockParamFloat _vAccel;     /**< accelerometer process noise  */
-	control::BlockParamFloat _rMag;       /**< magnetometer measurement noise  */
-	control::BlockParamFloat _rGpsVel;    /**< gps velocity measurement noise */
-	control::BlockParamFloat _rGpsPos;    /**< gps position measurement noise */
-	control::BlockParamFloat _rGpsAlt;    /**< gps altitude measurement noise */
-	control::BlockParamFloat _rPressAlt;  /**< press altitude measurement noise */
-	control::BlockParamFloat _rAccel;     /**< accelerometer measurement noise */
-	control::BlockParamFloat _magDip;     /**< magnetic inclination with level */
-	control::BlockParamFloat _magDec;     /**< magnetic declination, clockwise rotation */
-	control::BlockParamFloat _g;          /**< gravitational constant */
-	control::BlockParamFloat _faultPos;   /**< fault detection threshold for position */
-	control::BlockParamFloat _faultAtt;   /**< fault detection threshold for attitude */
-	// status
-	bool _attitudeInitialized;
-	bool _positionInitialized;
-	uint16_t _attitudeInitCounter;
-	// accessors
-	int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
-	void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
-	int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
-	void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
-	int32_t getAltE3() { return int32_t(alt * 1.0e3); }
-	void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
-};
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
deleted file mode 100644
index 372b2d162..000000000
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ /dev/null
@@ -1,157 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: 	James Goppert
- *
- * 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 kalman_main.cpp
- * Combined attitude / position estimator.
- *
- * @author James Goppert
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-#include <math.h>
-#include "KalmanNav.hpp"
-
-static bool thread_should_exit = false;     /**< Deamon exit flag */
-static bool thread_running = false;     /**< Deamon status flag */
-static int daemon_task;             /**< Handle of deamon task / thread */
-
-/**
- * Deamon management function.
- */
-extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
-
-/**
- * Mainloop of deamon.
- */
-int kalman_demo_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int att_pos_estimator_ekf_main(int argc, char *argv[])
-{
-
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-
-		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 30,
-					 4096,
-					 kalman_demo_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("is running\n");
-			exit(0);
-
-		} else {
-			warnx("not started\n");
-			exit(1);
-		}
-
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int kalman_demo_thread_main(int argc, char *argv[])
-{
-
-	warnx("starting");
-
-	using namespace math;
-
-	thread_running = true;
-
-	KalmanNav nav(NULL, "KF");
-
-	while (!thread_should_exit) {
-		nav.update();
-	}
-
-	warnx("exiting.");
-
-	thread_running = false;
-
-	return 0;
-}
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 a70a14fe4..35dc39ec6 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.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
@@ -34,9 +32,12 @@
  ****************************************************************************/
 
 /*
- * @file attitude_estimator_ekf_main.c
+ * @file attitude_estimator_ekf_main.cpp
  *
  * Extended Kalman Filter for Attitude Estimation.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -58,9 +59,14 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -107,7 +113,7 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
- * to task_create().
+ * to task_spawn_cmd().
  */
 int attitude_estimator_ekf_main(int argc, char *argv[])
 {
@@ -214,6 +220,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_control_mode_s control_mode;
@@ -221,12 +231,32 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
+	uint64_t last_vel_t = 0;
+
+	/* current velocity */
+	math::Vector<3> vel;
+	vel.zero();
+	/* previous velocity */
+	math::Vector<3> vel_prev;
+	vel_prev.zero();
+	/* actual acceleration (by GPS velocity) in body frame */
+	math::Vector<3> acc;
+	acc.zero();
+	/* rotation matrix */
+	math::Matrix<3, 3> R;
+	R.identity();
 
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
 	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
 	orb_set_interval(sub_raw, 3);
 
+	/* subscribe to GPS */
+	int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	/* subscribe to GPS */
+	int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));
+
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
@@ -237,7 +267,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 	int loopcounter = 0;
-	int printcounter = 0;
 
 	thread_running = true;
 
@@ -245,11 +274,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	// orb_advert_t pub_dbg = -1;
 
-	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
-	// XXX write this out to perf regs
-
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_ekf_params ekf_params;
@@ -259,11 +284,16 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
 
-	uint64_t start_time = hrt_absolute_time();
 	bool initialized = false;
 
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
-	unsigned offset_count = 0;
+
+	/* magnetic declination, in radians */
+	float mag_decl = 0.0f;
+
+	/* rotation matrix for magnetic declination */
+	math::Matrix<3, 3> R_decl;
+	R_decl.identity();
 
 	/* register the perf counter */
 	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
@@ -307,6 +337,25 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				/* get latest measurements */
 				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
 
+				bool gps_updated;
+				orb_check(sub_gps, &gps_updated);
+				if (gps_updated) {
+					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
+
+					if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
+
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, mag_decl);
+					}
+				}
+
+				bool global_pos_updated;
+				orb_check(sub_global_pos, &global_pos_updated);
+				if (global_pos_updated) {
+					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
+				}
+
 				if (!initialized) {
 					// XXX disabling init for now
 					initialized = true;
@@ -333,10 +382,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
-						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
+						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
@@ -345,23 +393,62 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
-						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
+						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
-					z_k[3] = raw.accelerometer_m_s2[0];
-					z_k[4] = raw.accelerometer_m_s2[1];
-					z_k[5] = raw.accelerometer_m_s2[2];
+					hrt_abstime vel_t = 0;
+					bool vel_valid = false;
+					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;
+							vel(0) = gps.vel_n_m_s;
+							vel(1) = gps.vel_e_m_s;
+							vel(2) = gps.vel_d_m_s;
+						}
+
+					} 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;
+							vel(0) = global_pos.vel_n;
+							vel(1) = global_pos.vel_e;
+							vel(2) = global_pos.vel_d;
+						}
+					}
+
+					if (vel_valid) {
+						/* velocity is valid */
+						if (vel_t != 0) {
+							/* velocity updated */
+							if (last_vel_t != 0 && vel_t != last_vel_t) {
+								float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
+								/* calculate acceleration in body frame */
+								acc = R.transposed() * ((vel - vel_prev) / vel_dt);
+							}
+							last_vel_t = vel_t;
+							vel_prev = vel;
+						}
+
+					} else {
+						/* velocity is valid, reset acceleration */
+						acc.zero();
+						vel_prev.zero();
+						last_vel_t = 0;
+					}
+
+					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
+					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
+					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
-						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
+						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					z_k[6] = raw.magnetometer_ga[0];
@@ -389,6 +476,17 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
 
+						/* update mag declination rotation matrix */
+						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
+
+						} else {
+							mag_decl = ekf_params.mag_decl;
+						}
+
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, mag_decl);
+
 						x_aposteriori_k[0] = z_k[0];
 						x_aposteriori_k[1] = z_k[1];
 						x_aposteriori_k[2] = z_k[2];
@@ -410,8 +508,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					uint64_t timing_start = hrt_absolute_time();
-
 					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
 							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
 
@@ -425,7 +521,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					if (last_data > 0 && raw.timestamp - last_data > 12000)
+					if (last_data > 0 && raw.timestamp - last_data > 30000)
 						printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
 
 					last_data = raw.timestamp;
@@ -433,10 +529,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* send out */
 					att.timestamp = raw.timestamp;
 
-					// XXX Apply the same transformation to the rotation matrix
-					att.roll = euler[0] - ekf_params.roll_off;
-					att.pitch = euler[1] - ekf_params.pitch_off;
-					att.yaw = euler[2] - ekf_params.yaw_off;
+					att.roll = euler[0];
+					att.pitch = euler[1];
+					att.yaw = euler[2] + mag_decl;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
@@ -445,12 +540,20 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];
 
-					//att.yawspeed =z_k[2] ;
+					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
+					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
+					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
+
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
 
+					/* magnetic declination */
+
+					math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
+					R = R_decl * R_body;
+
 					/* copy rotation matrix */
-					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+					memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
 					att.R_valid = true;
 
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 3cfddf28e..bc0e3b93a 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -40,6 +40,7 @@
  */
 
 #include "attitude_estimator_ekf_params.h"
+#include <math.h>
 
 /* Extended Kalman Filter covariances */
 
@@ -60,10 +61,10 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
 /* offset estimation - UNUSED */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
-/* offsets in roll, pitch and yaw of sensor plane and body */
-PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
+/* magnetic declination, in degrees */
+PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
+
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
 
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
@@ -79,9 +80,9 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
 	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
-	h->roll_off  =	param_find("ATT_ROLL_OFF3");
-	h->pitch_off =	param_find("ATT_PITCH_OFF3");
-	h->yaw_off   =	param_find("ATT_YAW_OFF3");
+	h->mag_decl   =	param_find("ATT_MAG_DECL");
+
+	h->acc_comp   =	param_find("ATT_ACC_COMP");
 
 	return OK;
 }
@@ -99,9 +100,10 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->r2, &(p->r[2]));
 	param_get(h->r3, &(p->r[3]));
 
-	param_get(h->roll_off, &(p->roll_off));
-	param_get(h->pitch_off, &(p->pitch_off));
-	param_get(h->yaw_off, &(p->yaw_off));
+	param_get(h->mag_decl, &(p->mag_decl));
+	p->mag_decl *= M_PI_F / 180.0f;
+
+	param_get(h->acc_comp, &(p->acc_comp));
 
 	return OK;
 }
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index 09817d58e..5985541ca 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -47,12 +47,15 @@ struct attitude_estimator_ekf_params {
 	float roll_off;
 	float pitch_off;
 	float yaw_off;
+	float mag_decl;
+	int acc_comp;
 };
 
 struct attitude_estimator_ekf_param_handles {
 	param_t r0, r1, r2, r3;
 	param_t q0, q1, q2, q3, q4;
-	param_t roll_off, pitch_off, yaw_off;
+	param_t mag_decl;
+	param_t acc_comp;
 };
 
 /**
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk
index d98647f99..99d0c5bf2 100644
--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -50,3 +50,5 @@ SRCS		 = attitude_estimator_ekf_main.cpp \
 		   codegen/rtGetNaN.c \
 		   codegen/norm.c \
 		   codegen/cross.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
index e79726613..e49027e47 100755
--- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Hyon Lim <limhyon@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   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
@@ -36,6 +34,9 @@
  /*
  * @file attitude_estimator_so3_main.cpp
  *
+ * @author Hyon Lim <limhyon@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
  * Implementation of nonlinear complementary filters on the SO(3).
  * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
  * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
@@ -131,7 +132,7 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
- * to task_create().
+ * to task_spawn_cmd().
  */
 int attitude_estimator_so3_main(int argc, char *argv[])
 {
@@ -391,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
  */
 int attitude_estimator_so3_thread_main(int argc, char *argv[])
 {
-	const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
-
 	//! Time constant
 	float dt = 0.005f;
 	
@@ -437,15 +436,12 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 	int loopcounter = 0;
-	int printcounter = 0;
 
 	thread_running = true;
 
-	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_so3_params so3_comp_params;
@@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 						gyro_offsets[0] /= offset_count;
 						gyro_offsets[1] /= offset_count;
 						gyro_offsets[2] /= offset_count;
-						warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
+						warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]);
 					}
 
 				} else {
@@ -523,13 +519,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					/* Calculate data time difference in seconds */
 					dt = (raw.timestamp - last_measurement) / 1000000.0f;
 					last_measurement = raw.timestamp;
-					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
-						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
-						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
@@ -538,11 +530,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
-						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
-						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					acc[0] = raw.accelerometer_m_s2[0];
@@ -550,11 +539,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					acc[2] = raw.accelerometer_m_s2[2];
 
 					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
-						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
-						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					mag[0] = raw.magnetometer_ga[0];
@@ -572,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 						continue;
 					}
 
-					uint64_t timing_start = hrt_absolute_time();
-
 					// NOTE : Accelerometer is reversed.
 					// Because proper mount of PX4 will give you a reversed accelerometer readings.
 					NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
@@ -612,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 						/* due to inputs or numerical failure the output is invalid, skip it */
 						// Due to inputs or numerical failure the output is invalid
 						warnx("infinite euler angles, rotation matrix:");
-						warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
-						warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
-						warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
+						warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]);
+						warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]);
+						warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]);
 						// Don't publish anything
 						continue;
 					}
diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk
index e29bb16a6..f52715abb 100644
--- a/src/modules/attitude_estimator_so3/module.mk
+++ b/src/modules/attitude_estimator_so3/module.mk
@@ -6,3 +6,5 @@ MODULE_COMMAND	 = attitude_estimator_so3
 
 SRCS		 = attitude_estimator_so3_main.cpp \
 		   attitude_estimator_so3_params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 5eeca5a1a..7a4e7a766 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   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
@@ -131,6 +130,7 @@
 #include <fcntl.h>
 #include <sys/prctl.h>
 #include <math.h>
+#include <float.h>
 #include <mathlib/mathlib.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
@@ -158,6 +158,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 
 int do_accel_calibration(int mavlink_fd)
 {
+	int fd;
+
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
 	struct accel_scale accel_scale = {
@@ -172,7 +174,7 @@ int do_accel_calibration(int mavlink_fd)
 	int res = OK;
 
 	/* reset all offsets to zero and all scales to one */
-	int fd = open(ACCEL_DEVICE_PATH, 0);
+	fd = open(ACCEL_DEVICE_PATH, 0);
 	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 	close(fd);
 
@@ -194,15 +196,13 @@ int do_accel_calibration(int mavlink_fd)
 		int32_t board_rotation_int;
 		param_get(board_rotation_h, &(board_rotation_int));
 		enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
-		math::Matrix board_rotation(3, 3);
+		math::Matrix<3, 3> board_rotation;
 		get_rot_matrix(board_rotation_id, &board_rotation);
-		math::Matrix board_rotation_t = board_rotation.transpose();
-		math::Vector3 accel_offs_vec;
-		accel_offs_vec.set(&accel_offs[0]);
-		math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
-		math::Matrix accel_T_mat(3, 3);
-		accel_T_mat.set(&accel_T[0][0]);
-		math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
+		math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
+		math::Vector<3> accel_offs_vec(&accel_offs[0]);
+		math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
+		math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
+		math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
 
 		accel_scale.x_offset = accel_offs_rotated(0);
 		accel_scale.x_scale = accel_T_rotated(0, 0);
@@ -225,7 +225,7 @@ int do_accel_calibration(int mavlink_fd)
 
 	if (res == OK) {
 		/* apply new scaling and offsets */
-		int fd = open(ACCEL_DEVICE_PATH, 0);
+		fd = open(ACCEL_DEVICE_PATH, 0);
 		res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 		close(fd);
 
@@ -279,11 +279,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			}
 		}
 
-		if (old_done_count != done_count)
+		if (old_done_count != done_count) {
 			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
+		}
 
-		if (done)
+		if (done) {
 			break;
+		}
 
 		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
 				 (!data_collected[0]) ? "x+ " : "",
@@ -313,7 +315,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 				 (double)accel_ref[orient][2]);
 
 		data_collected[orient] = true;
-		tune_neutral();
+		tune_neutral(true);
 	}
 
 	close(sensor_combined_sub);
@@ -382,11 +384,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);
 
-				if (d > still_thr2 * 8.0f)
+				if (d > still_thr2 * 8.0f) {
 					d = still_thr2 * 8.0f;
+				}
 
-				if (d > accel_disp[i])
+				if (d > accel_disp[i]) {
 					accel_disp[i] = d;
+				}
 			}
 
 			/* still detector with hysteresis */
@@ -434,33 +438,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 
 	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
 	    fabsf(accel_ema[1]) < accel_err_thr &&
-	    fabsf(accel_ema[2]) < accel_err_thr)
-		return 0;	// [ g, 0, 0 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 0;        // [ g, 0, 0 ]
+	}
 
 	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
 	    fabsf(accel_ema[1]) < accel_err_thr &&
-	    fabsf(accel_ema[2]) < accel_err_thr)
-		return 1;	// [ -g, 0, 0 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 1;        // [ -g, 0, 0 ]
+	}
 
 	if (fabsf(accel_ema[0]) < accel_err_thr &&
 	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
-	    fabsf(accel_ema[2]) < accel_err_thr)
-		return 2;	// [ 0, g, 0 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 2;        // [ 0, g, 0 ]
+	}
 
 	if (fabsf(accel_ema[0]) < accel_err_thr &&
 	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
-	    fabsf(accel_ema[2]) < accel_err_thr)
-		return 3;	// [ 0, -g, 0 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 3;        // [ 0, -g, 0 ]
+	}
 
 	if (fabsf(accel_ema[0]) < accel_err_thr &&
 	    fabsf(accel_ema[1]) < accel_err_thr &&
-	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
-		return 4;	// [ 0, 0, g ]
+	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
+		return 4;        // [ 0, 0, g ]
+	}
 
 	if (fabsf(accel_ema[0]) < accel_err_thr &&
 	    fabsf(accel_ema[1]) < accel_err_thr &&
-	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
-		return 5;	// [ 0, 0, -g ]
+	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
+		return 5;        // [ 0, 0, -g ]
+	}
 
 	mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
 
@@ -487,8 +497,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 			struct sensor_combined_s sensor;
 			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-			for (int i = 0; i < 3; i++)
+			for (int i = 0; i < 3; i++) {
 				accel_sum[i] += sensor.accelerometer_m_s2[i];
+			}
 
 			count++;
 
@@ -497,8 +508,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 			continue;
 		}
 
-		if (errcount > samples_num / 10)
+		if (errcount > samples_num / 10) {
 			return ERROR;
+		}
 	}
 
 	for (int i = 0; i < 3; i++) {
@@ -514,8 +526,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
 		    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
 		    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
 
-	if (det == 0.0f)
-		return ERROR;	// Singular matrix
+	if (fabsf(det) < FLT_EPSILON) {
+		return ERROR;        // Singular matrix
+	}
 
 	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
 	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
@@ -551,8 +564,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 	/* calculate inverse matrix for A */
 	float mat_A_inv[3][3];
 
-	if (mat_invert3(mat_A, mat_A_inv) != OK)
+	if (mat_invert3(mat_A, mat_A_inv) != OK) {
 		return ERROR;
+	}
 
 	/* copy results to accel_T */
 	for (int i = 0; i < 3; i++) {
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index 1cf9c0977..6b7e16d44 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   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
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 1809f9688..598cfe9e2 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -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
@@ -51,6 +51,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
+#include <systemlib/airspeed.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -64,38 +65,58 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "don't move system");
 
-	const int calibration_count = 2500;
+	const unsigned calibration_count = 2000;
 
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
 
-	int calibration_counter = 0;
 	float diff_pres_offset = 0.0f;
 
 	/* Reset sensor parameters */
 	struct airspeed_scale airscale = {
-		0.0f,
+		diff_pres_offset,
 		1.0f,
 	};
 
 	bool paramreset_successful = false;
 	int  fd = open(AIRSPEED_DEVICE_PATH, 0);
+
 	if (fd > 0) {
 		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 			paramreset_successful = true;
+
+		} else {
+			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
 		}
+
 		close(fd);
 	}
 
 	if (!paramreset_successful) {
-		warn("WARNING: failed to set scale / offsets for airspeed sensor");
-		mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
-		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
-		return ERROR;
+
+		/* only warn if analog scaling is zero */
+		float analog_scaling = 0.0f;
+		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
+		if (fabsf(analog_scaling) < 0.1f) {
+			mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
+			close(diff_pres_sub);
+			return ERROR;
+		}
+
+		/* set scaling offset parameter */
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			close(diff_pres_sub);
+			return ERROR;
+		}
 	}
 
+	unsigned calibration_counter = 0;
+
+	mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
+	usleep(500 * 1000);
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -107,11 +128,13 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			diff_pres_offset += diff_pres.differential_pressure_pa;
+
+			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
 
-			if (calibration_counter % (calibration_count / 20) == 0)
-				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+			if (calibration_counter % (calibration_count / 20) == 0) {
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
+			}
 
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
@@ -125,6 +148,16 @@ int do_airspeed_calibration(int mavlink_fd)
 
 	if (isfinite(diff_pres_offset)) {
 
+		int  fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
+		airscale.offset_pa = diff_pres_offset;
+		if (fd_scale > 0) {
+			if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+				mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
+			}
+
+			close(fd_scale);
+		}
+
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			close(diff_pres_sub);
@@ -141,14 +174,91 @@ int do_airspeed_calibration(int mavlink_fd)
 			return ERROR;
 		}
 
-		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-		tune_neutral();
-		close(diff_pres_sub);
-		return OK;
-
 	} else {
 		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		close(diff_pres_sub);
 		return ERROR;
 	}
+
+	mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
+
+	/* wait 500 ms to ensure parameter propagated through the system */
+	usleep(500 * 1000);
+
+	calibration_counter = 0;
+	const int maxcount = 3000;
+
+	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
+	while (calibration_counter < maxcount) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
+			calibration_counter++;
+
+			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
+				if (calibration_counter % 100 == 0) {
+					mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				}
+				continue;
+			}
+
+			/* do not allow negative values */
+			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
+				mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
+				close(diff_pres_sub);
+
+				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
+
+				diff_pres_offset = 0.0f;
+				if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+					mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+					close(diff_pres_sub);
+					return ERROR;
+				}
+
+				/* save */
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
+				(void)param_save_default();
+
+				close(diff_pres_sub);
+
+				mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+				return ERROR;
+			} else {
+				mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				break;
+			}
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			close(diff_pres_sub);
+			return ERROR;
+		}
+	}
+
+	if (calibration_counter == maxcount) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		close(diff_pres_sub);
+		return ERROR;
+	}
+
+	mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+
+	mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+	tune_neutral(true);
+	close(diff_pres_sub);
+	return OK;
 }
diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h
index 71c701fc2..8c6b65ff1 100644
--- a/src/modules/commander/airspeed_calibration.h
+++ b/src/modules/commander/airspeed_calibration.h
@@ -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
diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp
index 3123c4087..da98644b4 100644
--- a/src/modules/commander/baro_calibration.cpp
+++ b/src/modules/commander/baro_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   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
diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h
index bc42bc6cf..ce78ae700 100644
--- a/src/modules/commander/baro_calibration.h
+++ b/src/modules/commander/baro_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   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
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
index fd8b8564d..b1e209efc 100644
--- a/src/modules/commander/calibration_messages.h
+++ b/src/modules/commander/calibration_messages.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   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
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index be38ea104..5796204bf 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012 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
@@ -40,6 +39,7 @@
  */
 
 #include <math.h>
+#include <float.h>
 
 #include "calibration_routines.h"
 
@@ -170,7 +170,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 	float aA, aB, aC, nA, nB, nC, dA, dB, dC;
 
 	//Iterate N times, ignore stop condition.
-	int n = 0;
+	unsigned int n = 0;
 
 	while (n < max_iterations) {
 		n++;
@@ -179,9 +179,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 		aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
 		aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
 		aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
-		aA = (aA == 0.0f) ? 1.0f : aA;
-		aB = (aB == 0.0f) ? 1.0f : aB;
-		aC = (aC == 0.0f) ? 1.0f : aC;
+		aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
+		aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
+		aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
 
 		//Compute next iteration
 		nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index e3e7fbafd..3c8e49ee9 100644
--- a/src/modules/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012 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
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 54219a34a..0c4d48dea 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1,11 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 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>
+ *   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
@@ -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>
@@ -52,6 +52,7 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <systemlib/err.h>
+#include <systemlib/circuit_breaker.h>
 #include <debug.h>
 #include <sys/prctl.h>
 #include <sys/stat.h>
@@ -67,15 +68,19 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -87,6 +92,8 @@
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
+#include <systemlib/state_table.h>
+#include <dataman/dataman.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -113,14 +120,16 @@ extern struct system_load_s system_load;
 
 #define MAVLINK_OPEN_INTERVAL 50000
 
-#define STICK_ON_OFF_LIMIT 0.75f
-#define STICK_THRUST_RANGE 1.0f
+#define STICK_ON_OFF_LIMIT 0.9f
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
-#define RC_TIMEOUT 100000
-#define DIFFPRESS_TIMEOUT 2000000
+#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 OFFBOARD_TIMEOUT		500000
+#define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
 #define PRINT_MODE_REJECT_INTERVAL	2000000
@@ -138,7 +147,7 @@ enum MAV_MODE_FLAG {
 };
 
 /* Mavlink file descriptors */
-static int mavlink_fd;
+static int mavlink_fd = 0;
 
 /* flags */
 static bool commander_initialized = false;
@@ -153,16 +162,14 @@ static uint64_t last_print_mode_reject_time = 0;
 static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
+static int parachute_enabled = 0;
+static float eph_epv_threshold = 5.0f;
 
 static struct vehicle_status_s status;
-
-/* armed topic */
 static struct actuator_armed_s armed;
-
 static struct safety_s safety;
-
-/* flags for control apps */
-struct vehicle_control_mode_s control_mode;
+static struct vehicle_control_mode_s control_mode;
+static struct offboard_control_setpoint_s sp_offboard;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -199,7 +206,7 @@ void usage(const char *reason);
 /**
  * React to commands that are sent e.g. from the mavlink module.
  */
-void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *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);
 
 /**
  * Mainloop of commander.
@@ -210,9 +217,11 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
+
+transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
 
-transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+void set_control_mode();
 
 void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
 
@@ -220,11 +229,10 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-int arm();
-int disarm();
-
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
+
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
@@ -235,8 +243,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 
 int commander_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
@@ -250,7 +259,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     3000,
+					     2950,
 					     commander_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 
@@ -263,8 +272,9 @@ int commander_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 
-		if (!thread_running)
+		if (!thread_running) {
 			errx(0, "commander already stopped");
+		}
 
 		thread_should_exit = true;
 
@@ -278,25 +288,32 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("\tcommander is running");
-			print_status();
+	/* commands needing the app to run below */
+	if (!thread_running) {
+		warnx("\tcommander not started");
+		exit(1);
+	}
 
-		} else {
-			warnx("\tcommander not started");
-		}
+	if (!strcmp(argv[1], "status")) {
+		print_status();
+		exit(0);
+	}
 
+	if (!strcmp(argv[1], "check")) {
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		int checkres = prearm_check(&status, mavlink_fd_local);
+		close(mavlink_fd_local);
+		warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm();
+		arm_disarm(true, mavlink_fd, "command line");
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
-		disarm();
+		arm_disarm(false, mavlink_fd, "command line");
 		exit(0);
 	}
 
@@ -306,8 +323,9 @@ int commander_main(int argc, char *argv[])
 
 void usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", reason);
+	}
 
 	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
 	exit(1);
@@ -315,6 +333,7 @@ void usage(const char *reason)
 
 void print_status()
 {
+	warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
 	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
 
 	/* read all relevant states */
@@ -364,277 +383,339 @@ void print_status()
 	warnx("arming: %s", armed_str);
 }
 
-static orb_advert_t control_mode_pub;
 static orb_advert_t status_pub;
 
-int arm()
+transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy)
 {
-	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
+	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
-	if (arming_res == TRANSITION_CHANGED) {
-		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
-		return 0;
-	} else {
-		return 1;
-	}
-}
+	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
+	// output appropriate error messages if the state cannot transition.
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
 
-int disarm()
-{
-	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
+		mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
 
-	if (arming_res == TRANSITION_CHANGED) {
-		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
-		return 0;
-	} else {
-		return 1;
+	} else if (arming_res == TRANSITION_DENIED) {
+		tune_negative(true);
 	}
+
+	return arming_res;
 }
 
-void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
+	struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
+	struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
 {
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+	/* only handle commands that are meant to be handled by this system and component */
+	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->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;
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-
-			/* 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, control_mode_pub, control_mode, mavlink_fd);
-
-			/* if HIL got enabled, reset battery status state */
-			if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
-				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
+			uint8_t base_mode = (uint8_t)cmd->param1;
+			uint8_t custom_main_mode = (uint8_t)cmd->param2;
 
-				if (arming_res != TRANSITION_DENIED) {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+			transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
 
-				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
-				}
-			}
-
-			// TODO remove debug code
-			//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
-			/* set arming state */
-			arming_res = TRANSITION_NOT_CHANGED;
-
-			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
-					print_reject_arm("NOT ARMING: Press safety switch first.");
-					arming_res = TRANSITION_DENIED;
-
-				} else {
-					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
-				}
+			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
-				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
-				}
-
-			} else {
-				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_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, control_mode, new_arming_state, armed);
-
-					if (arming_res == TRANSITION_CHANGED) {
-						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
-					}
-
-				} else {
-					arming_res = TRANSITION_NOT_CHANGED;
-				}
-			}
+			/* 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_local, 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 (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);
+					main_ret = main_state_transition(status_local, 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_ALTCTL) {
+					/* ALTCTL */
+					main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
 
-				} 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_POSCTL) {
+					/* POSCTL */
+					main_ret = main_state_transition(status_local, 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_local, MAIN_STATE_AUTO_MISSION);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
+					/* ACRO */
+					main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
+					/* OFFBOARD */
+					main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
 				}
 
 			} 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_local, MAIN_STATE_AUTO_MISSION);
 
 				} 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);
+						/* POSCTL */
+						main_ret = main_state_transition(status_local, 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_local, MAIN_STATE_MANUAL);
 					}
 				}
 			}
 
-			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+			if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
-
-			break;
 		}
+		break;
+
+	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
+			// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
+			// for logic state parameters
 
-	case VEHICLE_CMD_NAV_TAKEOFF: {
-			if (armed->armed) {
-				transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+			if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
+				mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
 
-				if (nav_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
+			} else {
+
+				bool cmd_arms = (static_cast<int>(cmd->param1 + 0.5f) == 1);
+
+				// Flick to inair restore first if this comes from an onboard system
+				if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
+					status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
 				}
 
-				if (nav_res != TRANSITION_DENIED) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
+
+				if (arming_res == TRANSITION_DENIED) {
+					mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd");
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 				}
+			}
+		}
+		break;
+
+	case VEHICLE_CMD_OVERRIDE_GOTO: {
+			// TODO listen vehicle_command topic directly from navigator (?)
+
+			// Increase by 0.5f and rely on the integer cast
+			// implicit floor(). This is the *safest* way to
+			// convert from floats representing small ints to actual ints.
+			unsigned int mav_goto = (cmd->param1 + 0.5f);
+
+			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
+				status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				mavlink_log_critical(mavlink_fd, "Pause mission cmd");
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
+				status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+				mavlink_log_critical(mavlink_fd, "Continue mission cmd");
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				/* reject TAKEOFF not armed */
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
+					(double)cmd->param1,
+					(double)cmd->param2,
+					(double)cmd->param3,
+					(double)cmd->param4,
+					(double)cmd->param5,
+					(double)cmd->param6,
+					(double)cmd->param7);
+			}
+		}
+		break;
+
+#if 0
+	/* Flight termination */
+	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
+
+			//XXX: to enable the parachute, a param needs to be set
+			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+			if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
+				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				/* reject parachute depoyment not armed */
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
-			break;
 		}
+		break;
+#endif
 
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+	case VEHICLE_CMD_DO_SET_HOME: {
+			bool use_current = cmd->param1 > 0.5f;
+
+			if (use_current) {
+				/* use current position */
+				if (status_local->condition_global_position_valid) {
+					home->lat = global_pos->lat;
+					home->lon = global_pos->lon;
+					home->alt = global_pos->alt;
 
-			if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
-				if (safety->safety_switch_available && !safety->safety_off) {
-					print_reject_arm("NOT ARMING: Press safety switch first.");
-					arming_res = TRANSITION_DENIED;
+					home->timestamp = hrt_absolute_time();
+
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
-					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 
-				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} else {
+				/* use specified position */
+				home->lat = cmd->param5;
+				home->lon = cmd->param6;
+				home->alt = cmd->param7;
+
+				home->timestamp = hrt_absolute_time();
+
+				cmd_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);
+
+				/* announce new home position */
+				if (*home_pub > 0) {
+					orb_publish(ORB_ID(home_position), *home_pub, home);
 
 				} else {
-					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					*home_pub = orb_advertise(ORB_ID(home_position), home);
 				}
+
+				/* mark home position as set */
+				status_local->condition_home_position_valid = true;
 			}
 		}
 		break;
 
+	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
+	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
+	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+		/* ignore commands that handled in low prio loop */
+		break;
+
 	default:
+		/* 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;
 	}
 
-	/* supported command handling stop */
-	if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-		tune_positive();
-
-	} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		/* we do not care in the high prio loop about commands we don't know */
-	} else {
-		tune_negative();
-
-		if (result == VEHICLE_CMD_RESULT_DENIED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
-
-		} else if (result == VEHICLE_CMD_RESULT_FAILED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
-
-		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
-
-		}
+	if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* already warned about unsupported commands in "default" case */
+		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
 	}
 
+	return true;
 }
 
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
 	commander_initialized = false;
-	bool home_position_set = false;
 
-	bool battery_tune_played = false;
 	bool arm_tune_played = false;
+	bool was_armed = false;
 
 	/* set parameters */
 	param_t _param_sys_type = param_find("MAV_TYPE");
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	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");
 
+	const char *main_states_str[MAIN_STATE_MAX];
+	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";
+	main_states_str[MAIN_STATE_OFFBOARD]			= "OFFBOARD";
+
+	const char *arming_states_str[ARMING_STATE_MAX];
+	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";
+
+	const 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_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";
+	nav_states_str[NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
+	nav_states_str[NAVIGATION_STATE_OFFBOARD]		= "OFFBOARD";
+
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
 
 	/* initialize */
 	if (led_init() != 0) {
-		warnx("ERROR: Failed to initialize leds");
+		warnx("ERROR: LED INIT FAIL");
 	}
 
 	if (buzzer_init() != OK) {
-		warnx("ERROR: Failed to initialize buzzer");
+		warnx("ERROR: BUZZER INIT FAIL");
 	}
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	/* Main state machine */
-	/* make sure we are in preflight state */
+	/* vehicle status topic */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
-
-	/* armed topic */
-	orb_advert_t armed_pub;
-	/* Initialize armed with all false */
-	memset(&armed, 0, sizeof(armed));
-
-	/* Initialize all flags to false */
-	memset(&control_mode, 0, sizeof(control_mode));
-
 	status.main_state = MAIN_STATE_MANUAL;
-	status.navigation_state = NAVIGATION_STATE_DIRECT;
+	status.nav_state = NAVIGATION_STATE_MANUAL;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
+	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -643,9 +724,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;
-
-	/* allow manual override initially */
-	control_mode.flag_external_manual_override_ok = true;
+	status.data_link_lost = true;
 
 	/* set battery warning flag */
 	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
@@ -654,40 +733,68 @@ int commander_thread_main(int argc, char *argv[])
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
 
-	// XXX just disable offboard control for now
-	control_mode.flag_control_offboard_enabled = false;
+	status.counter++;
+	status.timestamp = hrt_absolute_time();
 
-	/* advertise to ORB */
-	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
-	/* publish current state machine */
+	status.condition_power_input_valid = true;
+	status.avionics_power_rail_voltage = -1.0f;
+
+	// CIRCUIT BREAKERS
+	status.circuit_breaker_engaged_power_check = false;
 
 	/* publish initial state */
-	status.counter++;
-	status.timestamp = hrt_absolute_time();
-	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+	if (status_pub < 0) {
+		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
+		warnx("exiting.");
+		exit(ERROR);
+	}
 
-	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	/* armed topic */
+	orb_advert_t armed_pub;
+	/* Initialize armed with all false */
+	memset(&armed, 0, sizeof(armed));
 
-	control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
+	/* vehicle control mode topic */
+	memset(&control_mode, 0, sizeof(control_mode));
+	orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
+
+	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
 	/* home position */
 	orb_advert_t home_pub = -1;
 	struct home_position_s home;
 	memset(&home, 0, sizeof(home));
 
-	if (status_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
-	}
+	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
+	orb_advert_t mission_pub = -1;
+	mission_s mission;
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
+			warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq);
+			mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
+												mission.dataman_id, mission.count, mission.current_seq);
+		} else {
+			const char *missionfail = "reading mission state failed";
+			warnx("%s", missionfail);
+			mavlink_log_critical(mavlink_fd, missionfail);
+
+			/* initialize mission state in dataman */
+			mission.dataman_id = 0;
+			mission.count = 0;
+			mission.current_seq = 0;
+			dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
+		}
 
-	mavlink_log_info(mavlink_fd, "[cmd] started");
+		mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+		orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
+	}
 
 	int ret;
 
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2900);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -706,15 +813,15 @@ int commander_thread_main(int argc, char *argv[])
 	bool low_battery_voltage_actions_done = false;
 	bool critical_battery_voltage_actions_done = false;
 
-	uint64_t last_idle_time = 0;
-	uint64_t start_time = 0;
+	hrt_abstime last_idle_time = 0;
+	hrt_abstime start_time = 0;
 
 	bool status_changed = true;
 	bool param_init_forced = true;
 
 	bool updated = false;
 
-	bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
+	rc_calibration_check(mavlink_fd);
 
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -722,6 +829,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;
@@ -729,13 +841,26 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* Subscribe to offboard control data */
 	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
+	/* Subscribe to telemetry status topics */
+	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
+	uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
+	bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
+
+	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+		telemetry_last_heartbeat[i] = 0;
+		telemetry_lost[i] = true;
+	}
+
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	struct vehicle_global_position_s global_position;
 	memset(&global_position, 0, sizeof(global_position));
+	/* Init EPH and EPV */
+	global_position.eph = 1000.0f;
+	global_position.epv = 1000.0f;
 
 	/* Subscribe to local position data */
 	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -783,6 +908,16 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));
 
+	/* Subscribe to position setpoint triplet */
+	int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	struct position_setpoint_triplet_s pos_sp_triplet;
+	memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+
+	/* Subscribe to system power */
+	int system_power_sub = orb_subscribe(ORB_ID(system_power));
+	struct system_power_s system_power;
+	memset(&system_power, 0, sizeof(system_power));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -791,6 +926,15 @@ int commander_thread_main(int argc, char *argv[])
 
 	start_time = hrt_absolute_time();
 
+	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;
+	bool failsafe_old = false;
+
 	while (!thread_should_exit) {
 
 		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
@@ -798,6 +942,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);
 
@@ -819,25 +966,28 @@ int commander_thread_main(int argc, char *argv[])
 				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
 				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
 				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
-					control_mode.flag_external_manual_override_ok = false;
 					status.is_rotary_wing = true;
 
 				} else {
-					control_mode.flag_external_manual_override_ok = true;
 					status.is_rotary_wing = false;
 				}
 
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
+
+				status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+
 				status_changed = true;
 
 				/* re-check RC calibration */
-				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
-
-				/* navigation parameters */
-				param_get(_param_takeoff_alt, &takeoff_alt);
+				rc_calibration_check(mavlink_fd);
 			}
+
+			/* 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);
@@ -852,6 +1002,41 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
 
+		if (sp_offboard.timestamp != 0 &&
+		    sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
+			if (status.offboard_control_signal_lost) {
+				status.offboard_control_signal_lost = false;
+				status_changed = true;
+			}
+		} else {
+			if (!status.offboard_control_signal_lost) {
+				status.offboard_control_signal_lost = true;
+				status_changed = true;
+			}
+		}
+
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			orb_check(telemetry_subs[i], &updated);
+
+			if (updated) {
+				struct telemetry_status_s telemetry;
+				memset(&telemetry, 0, sizeof(telemetry));
+
+				orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
+
+				/* perform system checks when new telemetry link connected */
+				if (mavlink_fd &&
+					telemetry_last_heartbeat[i] == 0 &&
+					telemetry.heartbeat_time > 0 &&
+					hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
+
+					(void)rc_calibration_check(mavlink_fd);
+				}
+
+				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
+			}
+		}
+
 		orb_check(sensor_sub, &updated);
 
 		if (updated) {
@@ -864,6 +1049,26 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
 		}
 
+		orb_check(system_power_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
+
+			if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
+				if (system_power.servo_valid &&
+					!system_power.brick_valid &&
+					!system_power.usb_connected) {
+					/* flying only on servo rail, this is unsafe */
+					status.condition_power_input_valid = false;
+				} else {
+					status.condition_power_input_valid = true;
+				}
+
+				/* copy avionics voltage */
+				status.avionics_power_rail_voltage = system_power.voltage5V_v;
+			}
+		}
+
 		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
 		/* update safety topic */
@@ -872,11 +1077,15 @@ int commander_thread_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
-			// XXX this would be the right approach to do it, but do we *WANT* this?
-			// /* disarm if safety is now on and still armed */
-			// if (safety.safety_switch_available && !safety.safety_off) {
-			// 	(void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
-			// }
+			/* disarm if safety is now on and still armed */
+			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
+				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
+					mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
+					arming_state_changed = true;
+				}
+			}
 		}
 
 		/* update global position estimate */
@@ -887,9 +1096,6 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
 		}
 
-		/* update condition_global_position_valid */
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
-
 		/* update local position estimate */
 		orb_check(local_position_sub, &updated);
 
@@ -898,20 +1104,92 @@ int commander_thread_main(int argc, char *argv[])
 			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;
+
+		if (status.condition_global_position_valid) {
+			if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
+				eph_epv_good = false;
+
+			} else {
+				eph_epv_good = true;
+			}
+
+		} else {
+			if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
+				eph_epv_good = true;
+
+			} else {
+				eph_epv_good = false;
+			}
+		}
+
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
+
+		/* check if GPS fix is ok */
+
+		/* update home position */
+		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
+		    (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+
+			home.lat = global_position.lat;
+			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);
+
+			/* announce new home position */
+			if (home_pub > 0) {
+				orb_publish(ORB_ID(home_position), home_pub, &home);
+
+			} else {
+				home_pub = orb_advertise(ORB_ID(home_position), &home);
+			}
+
+			/* mark home position as set */
+			status.condition_home_position_valid = true;
+			tune_positive(true);
+		}
+
 		/* update condition_local_position_valid and condition_local_altitude_valid */
-		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
+		/* hysteresis for EPH */
+		bool local_eph_good;
+
+		if (status.condition_global_position_valid) {
+			if (local_position.eph > eph_epv_threshold * 2.0f) {
+				local_eph_good = false;
+
+			} else {
+				local_eph_good = true;
+			}
+
+		} else {
+			if (local_position.eph < eph_epv_threshold) {
+				local_eph_good = true;
+
+			} else {
+				local_eph_good = false;
+			}
+		}
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_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);
 
-		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;
 
 				if (status.condition_landed) {
-					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
+					mavlink_log_critical(mavlink_fd, "LANDED MODE");
 
 				} else {
-					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
+					mavlink_log_critical(mavlink_fd, "IN AIR MODE");
 				}
 			}
 		}
@@ -921,6 +1199,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
 			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
@@ -965,12 +1244,20 @@ int commander_thread_main(int argc, char *argv[])
 			status_changed = true;
 		}
 
+		/* update position setpoint triplet */
+		orb_check(pos_sp_triplet_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
+		}
+
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 			/* compute system load */
 			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
 
-			if (last_idle_time > 0)
-				status.load = 1.0f - ((float)interval_runtime / 1e6f);	//system load is time spent in non-idle
+			if (last_idle_time > 0) {
+				status.load = 1.0f - ((float)interval_runtime / 1e6f);        //system load is time spent in non-idle
+			}
 
 			last_idle_time = system_load.tasks[0].total_runtime;
 
@@ -982,25 +1269,30 @@ int commander_thread_main(int argc, char *argv[])
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
-			mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
+			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
-			battery_tune_played = false;
 
 		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
-			mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
+			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
-			battery_tune_played = false;
 
 			if (armed.armed) {
-				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+
+				if (arming_ret == TRANSITION_CHANGED) {
+					arming_state_changed = true;
+				}
 
 			} else {
-				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
-			}
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
 
+				if (arming_ret == TRANSITION_CHANGED) {
+					arming_state_changed = true;
+				}
+			}
 			status_changed = true;
 		}
 
@@ -1008,11 +1300,15 @@ 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, &control_mode, ARMING_STATE_STANDBY, &armed);
+			/* TODO: check for sensors */
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+
+			if (arming_ret == TRANSITION_CHANGED) {
+				arming_state_changed = true;
+			}
 
 		} else {
-			// XXX: Add emergency stuff if sensors are lost
+			/* TODO: Add emergency stuff if sensors are lost */
 		}
 
 
@@ -1029,163 +1325,152 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-			/* check if GPS fix is ok */
-			float hdop_threshold_m = 4.0f;
-			float vdop_threshold_m = 8.0f;
-
-			/*
-			 * If horizontal dilution of precision (hdop / eph)
-			 * and vertical diluation of precision (vdop / epv)
-			 * are below a certain threshold (e.g. 4 m), AND
-			 * home position is not yet set AND the last GPS
-			 * GPS measurement is not older than two seconds AND
-			 * the system is currently not armed, set home
-			 * position to the current position.
-			 */
-
-			if (!home_position_set && gps_position.fix_type >= 3 &&
-			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
-			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
-				/* copy position data to uORB home message, store it locally as well */
-				// TODO use global position estimate
-				home.lat = gps_position.lat;
-				home.lon = gps_position.lon;
-				home.alt = gps_position.alt;
-
-				home.eph_m = gps_position.eph_m;
-				home.epv_m = gps_position.epv_m;
-
-				home.s_variance_m_s = gps_position.s_variance_m_s;
-				home.p_variance_m = gps_position.p_variance_m;
-
-				double home_lat_d = home.lat * 1e-7;
-				double home_lon_d = home.lon * 1e-7;
-				warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
-
-				/* announce new home position */
-				if (home_pub > 0) {
-					orb_publish(ORB_ID(home_position), home_pub, &home);
+		}
 
-				} else {
-					home_pub = orb_advertise(ORB_ID(home_position), &home);
-				}
+		orb_check(mission_result_sub, &updated);
 
-				/* mark home position as set */
-				home_position_set = true;
-				tune_positive();
-			}
+		if (updated) {
+			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
 		}
 
-		/* ignore RC signals if in offboard control mode */
-		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0 && !status.rc_input_blocked) {
-			/* start RC input check */
-			if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
-				/* handle the case where RC signal was regained */
-				if (!status.rc_signal_found_once) {
-					status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
-					status_changed = true;
+		/* 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) {
+				status.rc_signal_found_once = true;
+				mavlink_log_critical(mavlink_fd, "detected RC signal first time");
+				status_changed = true;
 
-				} else {
-					if (status.rc_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
-						status_changed = true;
-					}
+			} else {
+				if (status.rc_signal_lost) {
+					mavlink_log_critical(mavlink_fd, "RC signal regained");
+					status_changed = true;
 				}
+			}
 
-				status.rc_signal_lost = false;
-
-				transition_result_t res;	// store all transitions results here
-
-				/* arm/disarm by RC */
-				res = TRANSITION_NOT_CHANGED;
-
-				/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
-				 * do it only for rotary wings */
-				if (status.is_rotary_wing &&
-				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
-				     (status.condition_landed && (
-					      status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
-					      status.navigation_state == NAVIGATION_STATE_VECTOR
-				      ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					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);
-						res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
-						stick_off_counter = 0;
-
-					} else {
-						stick_off_counter++;
+			status.rc_signal_lost = false;
+
+			/* 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 &&
+			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
+			    (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
+			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
+
+				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_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+					if (arming_ret == TRANSITION_CHANGED) {
+						arming_state_changed = true;
 					}
+					stick_off_counter = 0;
 
 				} else {
-					stick_off_counter = 0;
+					stick_off_counter++;
 				}
 
-				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
-				if (status.arming_state == ARMING_STATE_STANDBY &&
-				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						if (safety.safety_switch_available && !safety.safety_off) {
-							print_reject_arm("NOT ARMING: Press safety switch first.");
-
-						} else if (status.main_state != MAIN_STATE_MANUAL) {
-							print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
-
-						} else {
-							res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
-						}
-
-						stick_on_counter = 0;
+			} else {
+				stick_off_counter = 0;
+			}
 
+			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
+			if (status.arming_state == ARMING_STATE_STANDBY &&
+			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
+				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+
+					/* we check outside of the transition function here because the requirement
+					 * for being in manual mode only applies to manual arming actions.
+					 * the system can be armed in auto if armed via the GCS.
+					 */
+					if (status.main_state != MAIN_STATE_MANUAL) {
+						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 					} else {
-						stick_on_counter++;
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+						if (arming_ret == TRANSITION_CHANGED) {
+							arming_state_changed = true;
+						}
 					}
 
-				} else {
 					stick_on_counter = 0;
+
+				} else {
+					stick_on_counter++;
 				}
 
-				if (res == TRANSITION_CHANGED) {
-					if (status.arming_state == ARMING_STATE_ARMED) {
-						mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+			} else {
+				stick_on_counter = 0;
+			}
 
-					} else {
-						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
-					}
+			if (arming_ret == TRANSITION_CHANGED) {
+				if (status.arming_state == ARMING_STATE_ARMED) {
+					mavlink_log_info(mavlink_fd, "ARMED by RC");
 
-				} else if (res == TRANSITION_DENIED) {
-					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				} else {
+					mavlink_log_info(mavlink_fd, "DISARMED by RC");
 				}
+				arming_state_changed = true;
+
+			} else if (arming_ret == TRANSITION_DENIED) {
+				/* DENIED here indicates bug in the commander */
+				mavlink_log_critical(mavlink_fd, "arming state transition denied");
+				tune_negative(true);
+			}
 
-				/* fill current_status according to mode switches */
-				check_mode_switches(&sp_man, &status);
+			/* evaluate the main state machine according to mode switches */
+			transition_result_t main_res = set_main_state_rc(&status, &sp_man);
 
-				/* evaluate the main state machine */
-				res = check_main_state_machine(&status);
+			/* play tune on mode change only if armed, blink LED always */
+			if (main_res == TRANSITION_CHANGED) {
+				tune_positive(armed.armed);
+				main_state_changed = true;
 
-				if (res == TRANSITION_CHANGED) {
-					//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
-					tune_positive();
+			} else if (main_res == TRANSITION_DENIED) {
+				/* DENIED here indicates bug in the commander */
+				mavlink_log_critical(mavlink_fd, "main state transition denied");
+			}
 
-				} else if (res == TRANSITION_DENIED) {
-					/* DENIED here indicates bug in the commander */
-					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+		} else {
+			if (!status.rc_signal_lost) {
+				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
+				status.rc_signal_lost = true;
+				status_changed = true;
+			}
+		}
+
+		/* data links check */
+		bool have_link = false;
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
+				/* handle the case where data link was regained */
+				if (telemetry_lost[i]) {
+					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
+					telemetry_lost[i] = false;
 				}
+				have_link = true;
 
 			} 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 (!telemetry_lost[i]) {
+					mavlink_log_critical(mavlink_fd, "data link %i lost", i);
+					telemetry_lost[i] = true;
 				}
 			}
 		}
 
+		if (have_link) {
+			/* handle the case where data link was regained */
+			if (status.data_link_lost) {
+				status.data_link_lost = false;
+				status_changed = true;
+			}
+
+		} else {
+			if (!status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
+				status.data_link_lost = true;
+				status_changed = true;
+			}
+		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
@@ -1195,40 +1480,83 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+			if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
+				status_changed = true;
+			}
 		}
 
-		/* evaluate the navigation state machine */
-		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
+		hrt_abstime t1 = hrt_absolute_time();
 
-		if (res == TRANSITION_DENIED) {
-			/* DENIED here indicates bug in the commander */
-			warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
-			mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+		/* print new state */
+		if (arming_state_changed) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
+
+			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
+			if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
+			    (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+
+				// TODO remove code duplication
+				home.lat = global_position.lat;
+				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, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+
+				/* announce new home position */
+				if (home_pub > 0) {
+					orb_publish(ORB_ID(home_position), home_pub, &home);
+
+				} else {
+					home_pub = orb_advertise(ORB_ID(home_position), &home);
+				}
+
+				/* mark home position as set */
+				status.condition_home_position_valid = true;
+			}
+			arming_state_changed = false;
 		}
 
-		/* 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 navigation_state_changed = check_navigation_state_changed();
+		was_armed = armed.armed;
 
-		hrt_abstime t1 = hrt_absolute_time();
+		/* now set navigation state according to failsafe and main state */
+		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
+						       mission_result.finished);
 
-		if (navigation_state_changed || arming_state_changed) {
-			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
+		// 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 (status.failsafe != failsafe_old) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+			failsafe_old = status.failsafe;
 		}
 
-		if (arming_state_changed || main_state_changed || navigation_state_changed) {
-			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
+		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 */
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
-			status.timestamp = t1;
-			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+			set_control_mode();
 			control_mode.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+
+			status.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
 			armed.timestamp = t1;
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
@@ -1236,26 +1564,23 @@ int commander_thread_main(int argc, char *argv[])
 		/* play arming and battery warning tunes */
 		if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
 			/* play tune when armed */
-			if (tune_arm() == OK)
-				arm_tune_played = true;
-
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
-			/* play tune on battery warning */
-			if (tune_low_bat() == OK)
-				battery_tune_played = true;
+			set_tune(TONE_ARMING_WARNING_TUNE);
+			arm_tune_played = true;
 
 		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
 			/* play tune on battery critical */
-			if (tune_critical_bat() == OK)
-				battery_tune_played = true;
+			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (battery_tune_played) {
-			tune_stop();
-			battery_tune_played = false;
+		} 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);
+
+		} else {
+			set_tune(TONE_STOP_TUNE);
 		}
 
 		/* reset arm_tune_played when disarmed */
-		if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
+		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
 			arm_tune_played = false;
 		}
 
@@ -1305,6 +1630,7 @@ int commander_thread_main(int argc, char *argv[])
 	close(diff_pres_sub);
 	close(param_changed_sub);
 	close(battery_sub);
+	close(mission_pub);
 
 	thread_running = false;
 
@@ -1324,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed)
+control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed)
 {
 	/* driving rgbled */
 	if (changed) {
 		bool set_normal_color = false;
 
 		/* set mode */
-		if (status->arming_state == ARMING_STATE_ARMED) {
+		if (status_local->arming_state == ARMING_STATE_ARMED) {
 			rgbled_set_mode(RGBLED_MODE_ON);
 			set_normal_color = true;
 
-		} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
 			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 			rgbled_set_color(RGBLED_COLOR_RED);
 
-		} else if (status->arming_state == ARMING_STATE_STANDBY) {
+		} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
 			rgbled_set_mode(RGBLED_MODE_BREATHE);
 			set_normal_color = true;
 
@@ -1350,15 +1676,12 @@ 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_NONE) {
-				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
-					rgbled_set_color(RGBLED_COLOR_AMBER);
-				}
-
+			if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
+				rgbled_set_color(RGBLED_COLOR_AMBER);
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
 			} else {
-				if (status->condition_local_position_valid) {
+				if (status_local->condition_local_position_valid) {
 					rgbled_set_color(RGBLED_COLOR_GREEN);
 
 				} else {
@@ -1377,21 +1700,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 	} else if (actuator_armed->ready_to_arm) {
 		/* ready to arm, blink at 1Hz */
-		if (leds_counter % 20 == 0)
+		if (leds_counter % 20 == 0) {
 			led_toggle(LED_BLUE);
+		}
 
 	} else {
 		/* not ready to arm, blink at 10Hz */
-		if (leds_counter % 2 == 0)
+		if (leds_counter % 2 == 0) {
 			led_toggle(LED_BLUE);
+		}
 	}
 
 #endif
 
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
-	if (status->load > 0.95f) {
-		if (leds_counter % 2 == 0)
+	if (status_local->load > 0.95f) {
+		if (leds_counter % 2 == 0) {
 			led_toggle(LED_AMBER);
+		}
 
 	} else {
 		led_off(LED_AMBER);
@@ -1400,109 +1726,125 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 	leds_counter++;
 }
 
-void
-check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+transition_result_t
+set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man)
 {
-	/* main mode switch */
-	if (!isfinite(sp_man->mode_switch)) {
-		/* default to manual if signal is invalid */
-		current_status->mode_switch = MODE_SWITCH_MANUAL;
-
-	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mode_switch = MODE_SWITCH_AUTO;
+	/* set main state according to RC switches */
+	transition_result_t res = TRANSITION_DENIED;
 
-	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
-		current_status->mode_switch = MODE_SWITCH_MANUAL;
+	/* offboard switch overrides main switch */
+	if (sp_man->offboard_switch == SWITCH_POS_ON) {
+		res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+		if (res == TRANSITION_DENIED) {
+			print_reject_mode(status_local, "OFFBOARD");
 
-	} else {
-		current_status->mode_switch = MODE_SWITCH_ASSISTED;
+		} else {
+			return res;
+		}
 	}
 
-	/* land switch */
-	if (!isfinite(sp_man->return_switch)) {
-		current_status->return_switch = RETURN_SWITCH_NONE;
-
-	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
-		current_status->return_switch = RETURN_SWITCH_RETURN;
+	/* offboard switched off or denied, check main mode switch */
+	switch (sp_man->mode_switch) {
+	case SWITCH_POS_NONE:
+		res = TRANSITION_NOT_CHANGED;
+		break;
 
-	} else {
-		current_status->return_switch = RETURN_SWITCH_NONE;
-	}
+	case SWITCH_POS_OFF:		// MANUAL
+		if (sp_man->acro_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status_local, MAIN_STATE_ACRO);
 
-	/* assisted switch */
-	if (!isfinite(sp_man->assisted_switch)) {
-		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+		} else {
+			res = main_state_transition(status_local, MAIN_STATE_MANUAL);
+		}
+		// TRANSITION_DENIED is not possible here
+		break;
 
-	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
-		current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+	case SWITCH_POS_MIDDLE:		// ASSIST
+		if (sp_man->posctl_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status_local, MAIN_STATE_POSCTL);
 
-	} else {
-		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
-	}
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
 
-	/* mission switch  */
-	if (!isfinite(sp_man->mission_switch)) {
-		current_status->mission_switch = MISSION_SWITCH_MISSION;
+			print_reject_mode(status_local, "POSCTL");
+		}
 
-	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mission_switch = MISSION_SWITCH_NONE;
+        // fallback to ALTCTL
+		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
 
-	} else {
-		current_status->mission_switch = MISSION_SWITCH_MISSION;
-	}
-}
+		if (res != TRANSITION_DENIED) {
+			break;	// changed successfully or already in this mode
+		}
 
-transition_result_t
-check_main_state_machine(struct vehicle_status_s *current_status)
-{
-	/* evaluate the main state machine */
-	transition_result_t res = TRANSITION_DENIED;
+		if (sp_man->posctl_switch != SWITCH_POS_ON) {
+			print_reject_mode(status_local, "ALTCTL");
+		}
 
-	switch (current_status->mode_switch) {
-	case MODE_SWITCH_MANUAL:
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// fallback to MANUAL
+		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case MODE_SWITCH_ASSISTED:
-		if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
-			res = main_state_transition(current_status, MAIN_STATE_EASY);
+	case SWITCH_POS_ON:			// AUTO
+		if (sp_man->return_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
 
-			if (res != TRANSITION_DENIED)
+			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
+			}
 
-			// else fallback to SEATBELT
-			print_reject_mode(current_status, "EASY");
-		}
+            print_reject_mode(status_local, "AUTO_RTL");
 
-		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+            // fallback to LOITER if home position not set
+            res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
 
-		if (res != TRANSITION_DENIED)
-			break;	// changed successfully or already in this mode
+            if (res != TRANSITION_DENIED) {
+                break;  // changed successfully or already in this state
+            }
 
-		if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
-			print_reject_mode(current_status, "SEATBELT");
+		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
 
-		// else fallback to MANUAL
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-		// TRANSITION_DENIED is not possible here
-		break;
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
 
-	case MODE_SWITCH_AUTO:
-		res = main_state_transition(current_status, MAIN_STATE_AUTO);
+            print_reject_mode(status_local, "AUTO_LOITER");
 
-		if (res != TRANSITION_DENIED)
-			break;	// changed successfully or already in this state
+		} else {
+			res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+
+            print_reject_mode(status_local, "AUTO_MISSION");
+
+            // fallback to LOITER if home position not set
+            res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+
+            if (res != TRANSITION_DENIED) {
+                break;  // changed successfully or already in this state
+            }
+		}
+
+        // fallback to POSCTL
+        res = main_state_transition(status_local, MAIN_STATE_POSCTL);
+
+        if (res != TRANSITION_DENIED) {
+            break;  // changed successfully or already in this state
+        }
 
-		// else fallback to SEATBELT (EASY likely will not work too)
-		print_reject_mode(current_status, "AUTO");
-		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+		// fallback to ALTCTL
+		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
 
-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
+		}
 
-		// else fallback to MANUAL
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// fallback to MANUAL
+		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -1514,194 +1856,211 @@ check_main_state_machine(struct vehicle_status_s *current_status)
 }
 
 void
-print_reject_mode(struct vehicle_status_s *current_status, const char *msg)
+set_control_mode()
 {
-	hrt_abstime t = hrt_absolute_time();
+	/* 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_offboard_enabled = false;
 
-	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
-		last_print_mode_reject_time = t;
-		char s[80];
-		sprintf(s, "#audio: REJECT %s", msg);
-		mavlink_log_critical(mavlink_fd, s);
-
-		// only buzz if armed, because else we're driving people nuts indoors
-		// they really need to look at the leds as well.
-		if (current_status->arming_state == ARMING_STATE_ARMED) {
-			tune_negative();
-		} else {
+	switch (status.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;
 
-			// Always show the led indication
-			led_negative();
+	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 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 NAVIGATION_STATE_OFFBOARD:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_offboard_enabled = true;
+
+		switch (sp_offboard.mode) {
+		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+			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;
+		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			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 OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true; /* XXX: hack for now */
+			control_mode.flag_control_velocity_enabled = true;
+			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
+			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;
+		default:
+			control_mode.flag_control_rates_enabled = false;
+			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;
+
+	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_RTGS:
+		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;
+		control_mode.flag_control_rates_enabled = false;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_termination_enabled = true;
+		break;
+
+	default:
+		break;
 	}
 }
 
 void
-print_reject_arm(const char *msg)
+print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
 {
 	hrt_abstime t = hrt_absolute_time();
 
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
-		char s[80];
-		sprintf(s, "#audio: %s", msg);
-		mavlink_log_critical(mavlink_fd, s);
-		tune_negative();
+		mavlink_log_critical(mavlink_fd, "REJECT %s", msg);
+
+		/* only buzz if armed, because else we're driving people nuts indoors
+		they really need to look at the leds as well. */
+		tune_negative(armed.armed);
 	}
 }
 
-transition_result_t
-check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
+void
+print_reject_arm(const char *msg)
 {
-	transition_result_t res = TRANSITION_DENIED;
-
-	if (status->main_state == MAIN_STATE_AUTO) {
-		if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-			// TODO AUTO_LAND handling
-			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-				/* don't switch to other states until takeoff not completed */
-				// XXX: only respect the condition_landed when the local position is actually valid
-				if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
-					return TRANSITION_NOT_CHANGED;
-				}
-			}
-
-			if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
-				/* possibly on ground, switch to TAKEOFF if needed */
-				if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
-					return res;
-				}
-			}
-
-			/* switch to AUTO mode */
-			if (status->rc_signal_found_once && !status->rc_signal_lost) {
-				/* act depending on switches when manual control enabled */
-				if (status->return_switch == RETURN_SWITCH_RETURN) {
-					/* RTL */
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
-
-				} else {
-					if (status->mission_switch == MISSION_SWITCH_MISSION) {
-						/* MISSION */
-						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-
-					} else {
-						/* LOITER */
-						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
-					}
-				}
-
-			} else {
-				/* switch to MISSION when no RC control and first time in some AUTO mode  */
-				if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
-					res = TRANSITION_NOT_CHANGED;
-
-				} else {
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-				}
-			}
-
-		} else {
-			/* disarmed, always switch to AUTO_READY */
-			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
-		}
-
-	} else {
-		/* manual control modes */
-		if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
-			/* switch to failsafe mode */
-			bool manual_control_old = control_mode->flag_control_manual_enabled;
-
-			if (!status->condition_landed && status->condition_local_position_valid) {
-				/* in air: try to hold position if possible */
-				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
-
-			} else {
-				/* landed: don't try to hold position but land (if taking off) */
-				res = TRANSITION_DENIED;
-			}
-
-			if (res == TRANSITION_DENIED) {
-				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
-			}
-
-			control_mode->flag_control_manual_enabled = false;
-
-			if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
-				/* mark navigation state as changed to force immediate flag publishing */
-				set_navigation_state_changed();
-				res = TRANSITION_CHANGED;
-			}
-
-			if (res == TRANSITION_CHANGED) {
-				if (control_mode->flag_control_position_enabled) {
-					mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
-
-				} else {
-					if (status->condition_landed) {
-						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
-
-					} else {
-						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
-					}
-				}
-			}
-
-		} else {
-			switch (status->main_state) {
-			case MAIN_STATE_MANUAL:
-				res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
-				break;
-
-			case MAIN_STATE_SEATBELT:
-				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
-				break;
-
-			case MAIN_STATE_EASY:
-				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
-				break;
+	hrt_abstime t = hrt_absolute_time();
 
-			default:
-				break;
-			}
-		}
+	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+		last_print_mode_reject_time = t;
+		mavlink_log_critical(mavlink_fd, msg);
+		tune_negative(true);
 	}
-
-	return res;
 }
 
 void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
 {
 	switch (result) {
 	case VEHICLE_CMD_RESULT_ACCEPTED:
-			tune_positive();
+		tune_positive(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
-		tune_negative();
+		mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command);
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
-		mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
-		tune_negative();
+		mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command);
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
-		tune_negative();
+		/* this needs additional hints to the user - so let other messages pass and be spoken */
+		mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command);
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-		mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
-		tune_negative();
+		mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command);
+		tune_negative(true);
 		break;
 
 	default:
@@ -1731,8 +2090,9 @@ void *commander_low_prio_loop(void *arg)
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
 
 		/* timed out - periodic check for thread_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -1746,8 +2106,10 @@ void *commander_low_prio_loop(void *arg)
 		/* ignore commands the high-prio loop handles */
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
-		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
+		    cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
 			continue;
+		}
 
 		/* only handle low-priority commands here */
 		switch (cmd.command) {
@@ -1782,9 +2144,7 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-
-				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -1825,6 +2185,7 @@ void *commander_low_prio_loop(void *arg)
 					/* airspeed calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 					calib_ret = do_airspeed_calibration(mavlink_fd);
+
 				} else if ((int)(cmd.param4) == 0) {
 					/* RC calibration ended - have we been in one worth confirming? */
 					if (status.rc_input_blocked) {
@@ -1839,12 +2200,14 @@ void *commander_low_prio_loop(void *arg)
 
 				}
 
-				if (calib_ret == OK)
-					tune_positive();
-				else
-					tune_negative();
+				if (calib_ret == OK) {
+					tune_positive(true);
+
+				} else {
+					tune_negative(true);
+				}
 
-				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
 
 				break;
 			}
@@ -1862,11 +2225,13 @@ void *commander_low_prio_loop(void *arg)
 						mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
 
 						/* convenience as many parts of NuttX use negative errno */
-						if (ret < 0)
+						if (ret < 0) {
 							ret = -ret;
+						}
 
-						if (ret < 1000)
+						if (ret < 1000) {
 							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+						}
 
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
@@ -1882,11 +2247,13 @@ void *commander_low_prio_loop(void *arg)
 						mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
 
 						/* convenience as many parts of NuttX use negative errno */
-						if (ret < 0)
+						if (ret < 0) {
 							ret = -ret;
+						}
 
-						if (ret < 1000)
+						if (ret < 1000) {
 							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+						}
 
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
@@ -1896,11 +2263,11 @@ void *commander_low_prio_loop(void *arg)
 			}
 
 		case VEHICLE_CMD_START_RX_PAIR:
-		/* handled in the IO driver */
-		break;
+			/* handled in the IO driver */
+			break;
 
 		default:
-			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			/* don't answer on unsupported commands, it will be done in main loop */
 			break;
 		}
 
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 033e7dc88..d5fe122cb 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           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
@@ -37,6 +34,11 @@
 /**
  * @file commander_helper.cpp
  * Commander helper functions implementations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
  */
 
 #include <stdio.h>
@@ -45,6 +47,7 @@
 #include <stdbool.h>
 #include <fcntl.h>
 #include <math.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -81,11 +84,22 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
 	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
 }
 
-static int buzzer;
-static hrt_abstime blink_msg_end;
+static int buzzer = -1;
+static hrt_abstime blink_msg_end = 0;	// end time for currently blinking LED message, 0 if no blink message
+static hrt_abstime tune_end = 0;		// end time of currently played tune, 0 for repeating tunes or silence
+static int tune_current = TONE_STOP_TUNE;		// currently playing tune, can be interrupted after tune_end
+static unsigned int tune_durations[TONE_NUMBER_OF_TUNES];
 
 int buzzer_init()
 {
+	tune_end = 0;
+	tune_current = 0;
+	memset(tune_durations, 0, sizeof(tune_durations));
+	tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000;
+	tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000;
+	tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
+	tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
+
 	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (buzzer < 0) {
@@ -101,58 +115,68 @@ void buzzer_deinit()
 	close(buzzer);
 }
 
-void tune_error()
+void set_tune(int tune)
 {
-	ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
+	unsigned int new_tune_duration = tune_durations[tune];
+
+	/* don't interrupt currently playing non-repeating tune by repeating */
+	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
+		/* allow interrupting current non-repeating tune by the same tune */
+		if (tune != tune_current || new_tune_duration != 0) {
+			ioctl(buzzer, TONE_SET_ALARM, tune);
+		}
+
+		tune_current = tune;
+
+		if (new_tune_duration != 0) {
+			tune_end = hrt_absolute_time() + new_tune_duration;
+
+		} else {
+			tune_end = 0;
+		}
+	}
 }
 
-void tune_positive()
+/**
+ * Blink green LED and play positive tune (if use_buzzer == true).
+ */
+void tune_positive(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_GREEN);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
+
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_POSITIVE_TUNE);
+	}
 }
 
-void tune_neutral()
+/**
+ * Blink white LED and play neutral tune (if use_buzzer == true).
+ */
+void tune_neutral(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_WHITE);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
-}
 
-void tune_negative()
-{
-	led_negative();
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
+	}
 }
 
-void led_negative()
+/**
+ * Blink red LED and play negative tune (if use_buzzer == true).
+ */
+void tune_negative(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_RED);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-}
-
-int tune_arm()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
-}
-
-int tune_low_bat()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
-}
 
-int tune_critical_bat()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
-}
-
-void tune_stop()
-{
-	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
+	}
 }
 
 int blink_msg_state()
@@ -161,6 +185,7 @@ int blink_msg_state()
 		return 0;
 
 	} else if (hrt_absolute_time() > blink_msg_end) {
+		blink_msg_end = 0;
 		return 2;
 
 	} else {
@@ -168,8 +193,8 @@ int blink_msg_state()
 	}
 }
 
-static int leds;
-static int rgbleds;
+static int leds = -1;
+static int rgbleds = -1;
 
 int led_init()
 {
@@ -183,30 +208,26 @@ int led_init()
 		return ERROR;
 	}
 
-	/* the blue LED is only available on FMUv1 but not FMUv2 */
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-
-	if (ioctl(leds, LED_ON, LED_BLUE)) {
-		warnx("Blue LED: ioctl fail\n");
-		return ERROR;
-	}
+	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
+	(void)ioctl(leds, LED_ON, LED_BLUE);
 
-#endif
+	/* switch blue off */
+	led_off(LED_BLUE);
 
+	/* we consider the amber led mandatory */
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
 		warnx("Amber LED: ioctl fail\n");
 		return ERROR;
 	}
 
+	/* switch amber off */
+	led_off(LED_AMBER);
+
 	/* then try RGB LEDs, this can fail on FMUv1*/
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
 
 	if (rgbleds == -1) {
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-		errx(1, "Unable to open " RGBLED_DEVICE_PATH);
-#else
-		warnx("No RGB LED found");
-#endif
+		warnx("No RGB LED found at " RGBLED_DEVICE_PATH);
 	}
 
 	return 0;
@@ -239,22 +260,25 @@ int led_off(int led)
 void rgbled_set_color(rgbled_color_t color)
 {
 
-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+	}
 }
 
 void rgbled_set_mode(rgbled_mode_t mode)
 {
 
-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+	}
 }
 
 void rgbled_set_pattern(rgbled_pattern_t *pattern)
 {
 
-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+	}
 }
 
 float battery_remaining_estimate_voltage(float voltage, float discharged)
@@ -294,6 +318,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
 	if (bat_capacity > 0.0f) {
 		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
 		ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
+
 	} else {
 		/* else use voltage */
 		ret = remaining_voltage;
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index af25a5e97..a49c9e263 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -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>
+ *   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
@@ -36,6 +34,9 @@
 /**
  * @file commander_helper.h
  * Commander helper functions definitions
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef COMMANDER_HELPER_H_
@@ -54,16 +55,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
 int buzzer_init(void);
 void buzzer_deinit(void);
 
-void tune_error(void);
-void tune_positive(void);
-void tune_neutral(void);
-void tune_negative(void);
-int tune_arm(void);
-int tune_low_bat(void);
-int tune_critical_bat(void);
-void tune_stop(void);
-
-void led_negative();
+void set_tune(int tune);
+void tune_positive(bool use_buzzer);
+void tune_neutral(bool use_buzzer);
+void tune_negative(bool use_buzzer);
 
 int blink_msg_state();
 
@@ -83,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
  * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
  * else use simple estimate based on voltage.
  *
+ * @param voltage the current battery voltage
+ * @param discharged the discharged capacity
  * @return the estimated remaining capacity in 0..1
  */
 float battery_remaining_estimate_voltage(float voltage, float discharged);
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index e10b7f18d..4750f9d5c 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -39,18 +39,59 @@
  *
  * @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>
 #include <systemlib/param/param.h>
 
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f);
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
+
+/**
+ * Empty cell voltage.
+ *
+ * Defines the voltage where a single cell of the battery is considered empty.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
+
+/**
+ * Full cell voltage.
+ *
+ * Defines the voltage where a single cell of the battery is considered full.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+
+/**
+ * Number of cells.
+ *
+ * Defines the number of cells the attached battery consists of.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+
+/**
+ * Battery capacity.
+ *
+ * Defines the capacity of the attached battery.
+ *
+ * @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/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
index 6e72cf0d9..0abb84a82 100644
--- a/src/modules/commander/commander_tests/commander_tests.cpp
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
 
 int commander_tests_main(int argc, char *argv[])
 {
-	state_machine_helper_test();
-	//state_machine_test();
+	stateMachineHelperTest();
 
 	return 0;
 }
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 40bedd9f3..2e18c4284 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -49,13 +49,12 @@ public:
 	StateMachineHelperTest();
 	virtual ~StateMachineHelperTest();
 
-	virtual const char*	run_tests();
+	virtual void runTests(void);
 
 private:
-	const char*	arming_state_transition_test();
-	const char*	arming_state_transition_arm_disarm_test();
-	const char*	main_state_transition_test();
-	const char*	is_safe_test();
+	bool armingStateTransitionTest();
+	bool mainStateTransitionTest();
+	bool isSafeTest();
 };
 
 StateMachineHelperTest::StateMachineHelperTest() {
@@ -64,132 +63,393 @@ StateMachineHelperTest::StateMachineHelperTest() {
 StateMachineHelperTest::~StateMachineHelperTest() {
 }
 
-const char*
-StateMachineHelperTest::arming_state_transition_test()
+bool StateMachineHelperTest::armingStateTransitionTest(void)
 {
+    // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
+    // to simulate machine state prior to testing an arming state transition. This structure is also
+    // use to represent the expected machine state after the transition has been requested.
+    typedef struct {
+        arming_state_t  arming_state;   // vehicle_status_s.arming_state
+        bool            armed;          // actuator_armed_s.armed
+        bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
+    } ArmingTransitionVolatileState_t;
+    
+    // This structure represents a test case for arming_state_transition. It contains the machine
+    // state prior to transtion, the requested state to transition to and finally the expected
+    // machine state after transition.
+    typedef struct {
+        const char*                     assertMsg;                              // Text to show when test case fails
+        ArmingTransitionVolatileState_t current_state;                          // Machine state prior to transtion
+        hil_state_t                     hil_state;                              // Current vehicle_status_s.hil_state
+        bool                            condition_system_sensors_initialized;   // Current vehicle_status_s.condition_system_sensors_initialized
+        bool                            safety_switch_available;                // Current safety_s.safety_switch_available
+        bool                            safety_off;                             // Current safety_s.safety_off
+        arming_state_t                  requested_state;                        // Requested arming state to transition to
+        ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
+        transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
+    } ArmingTransitionTest_t;
+    
+    // We use these defines so that our test cases are more readable
+    #define ATT_ARMED true
+    #define ATT_DISARMED false
+    #define ATT_READY_TO_ARM true
+    #define ATT_NOT_READY_TO_ARM false
+    #define ATT_SENSORS_INITIALIZED true
+    #define ATT_SENSORS_NOT_INITIALIZED false
+    #define ATT_SAFETY_AVAILABLE true
+    #define ATT_SAFETY_NOT_AVAILABLE true
+    #define ATT_SAFETY_OFF true
+    #define ATT_SAFETY_ON false
+    
+    // These are test cases for arming_state_transition
+    static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
+        // TRANSITION_NOT_CHANGED tests
+        
+        { "no transition: identical states",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
+        
+        // TRANSITION_CHANGED tests
+        
+        // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
+        
+        { "transition: init to standby",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to standby error",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to reboot",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to init",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to standby error",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to reboot",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed to standby",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed to armed error",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED_ERROR,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed error to standby error",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby error to reboot",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: in air restore to armed",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: in air restore to reboot",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // hil on tests, standby error to standby not normally allowed
+        
+        { "transition: standby error to standby, hil on",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // Safety switch arming tests
+        
+        { "transition: standby to armed, no safety switch",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to armed, safety switch off",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // standby error
+        { "transition: armed error to standby error requested standby",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // TRANSITION_DENIED tests
+        
+        // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
+        
+        { "no transition: init to armed",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby to armed error",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED_ERROR,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed to init",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed to reboot",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed error to armed",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed error to reboot",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby error to armed",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby error to standby",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: reboot to armed",
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: in air restore to standby",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        // Sensor tests
+        
+        { "no transition: init to standby - sensors not initialized",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        // Safety switch arming tests
+        
+        { "no transition: init to standby, safety switch on",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+    };
+    
 	struct vehicle_status_s status;
-	struct safety_s safety;
-	arming_state_t new_arming_state;
-	struct actuator_armed_s armed;
-
-	// Identical states.
-	status.arming_state = ARMING_STATE_INIT;
-	new_arming_state = ARMING_STATE_INIT;
-	mu_assert("no transition: identical states",
-		  TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-
-	// INIT to STANDBY.
-	armed.armed = false;
-	armed.ready_to_arm = false;
-	status.arming_state = ARMING_STATE_INIT;
-	status.condition_system_sensors_initialized = true;
-	new_arming_state = ARMING_STATE_STANDBY;
-	mu_assert("transition: init to standby",
-		  TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-	mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
-	mu_assert("not armed", !armed.armed);
-	mu_assert("ready to arm", armed.ready_to_arm);
-
-	// INIT to STANDBY, sensors not initialized.
-	armed.armed = false;
-	armed.ready_to_arm = false;
-	status.arming_state = ARMING_STATE_INIT;
-	status.condition_system_sensors_initialized = false;
-	new_arming_state = ARMING_STATE_STANDBY;
-	mu_assert("no transition: sensors not initialized",
-		  TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-	mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
-	mu_assert("not armed", !armed.armed);
-	mu_assert("not ready to arm", !armed.ready_to_arm);
-
-	return 0;
-}
-
-const char*
-StateMachineHelperTest::arming_state_transition_arm_disarm_test()
-{
-	struct vehicle_status_s status;
-	struct safety_s safety;
-	arming_state_t new_arming_state;
+	struct safety_s         safety;
 	struct actuator_armed_s armed;
-
-	// TODO(sjwilks): ARM then DISARM.
-	return 0;
+    
+    size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
+    for (size_t i=0; i<cArmingTransitionTests; i++) {
+        const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
+        
+        // Setup initial machine state
+        status.arming_state = test->current_state.arming_state;
+        status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
+        status.hil_state = test->hil_state;
+        safety.safety_switch_available = test->safety_switch_available;
+        safety.safety_off = test->safety_off;
+        armed.armed = test->current_state.armed;
+        armed.ready_to_arm = test->current_state.ready_to_arm;
+        
+        // Attempt transition
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
+        
+        // Validate result of transition
+        ut_assert(test->assertMsg, test->expected_transition_result == result);
+        ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
+        ut_assert(test->assertMsg, armed.armed == test->expected_state.armed);
+        ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm);
+    }
+
+	return true;
 }
 
-const char*
-StateMachineHelperTest::main_state_transition_test()
+bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
-	struct vehicle_status_s current_state;
-	main_state_t new_main_state;
+	// This structure represent a single test case for testing Main State transitions.
+	typedef struct {
+		const char*     assertMsg;				// Text to show when test case fails
+		uint8_t		condition_bits;				// Bits for various condition_* values
+		main_state_t	from_state;				// State prior to transition request
+		main_state_t	to_state;				// State to transition to
+		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
+	} MainTransitionTest_t;
+	
+	// Bits for condition_bits
+	#define MTT_ALL_NOT_VALID		0
+	#define MTT_ROTARY_WING			1 << 0
+	#define MTT_LOC_ALT_VALID		1 << 1
+	#define MTT_LOC_POS_VALID		1 << 2
+	#define MTT_HOME_POS_VALID		1 << 3
+	#define MTT_GLOBAL_POS_VALID		1 << 4
 	
-	// Identical states.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	new_main_state = MAIN_STATE_MANUAL;
-	mu_assert("no transition: identical states",
-		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
-
-	// AUTO to MANUAL.
-	current_state.main_state = MAIN_STATE_AUTO;
-	new_main_state = MAIN_STATE_MANUAL;
-	mu_assert("transition changed: auto to manual",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to SEATBELT.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_altitude_valid = true;
-	new_main_state = MAIN_STATE_SEATBELT;
-	mu_assert("tranisition: manual to seatbelt", 
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
-
-	// MANUAL to SEATBELT, invalid local altitude.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_altitude_valid = false;
-	new_main_state = MAIN_STATE_SEATBELT;
-	mu_assert("no transition: invalid local altitude",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to EASY.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_position_valid = true;
-	new_main_state = MAIN_STATE_EASY;
-	mu_assert("transition: manual to easy",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
-
-	// MANUAL to EASY, invalid local position.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_position_valid = false;
-	new_main_state = MAIN_STATE_EASY;
-	mu_assert("no transition: invalid position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to AUTO.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_global_position_valid = true;
-	new_main_state = MAIN_STATE_AUTO;
-	mu_assert("transition: manual to auto",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
-
-	// MANUAL to AUTO, invalid global position.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_global_position_valid = false;
-	new_main_state = MAIN_STATE_AUTO;
-	mu_assert("no transition: invalid global position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	return 0;
+	static const MainTransitionTest_t rgMainTransitionTests[] = {
+		
+		// TRANSITION_NOT_CHANGED tests
+		
+		{ "no transition: identical states",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+				
+		// TRANSITION_CHANGED tests
+		
+		{ "transition: MANUAL to ACRO",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
+
+		{ "transition: ACRO to MANUAL",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_MISSION - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_MISSION to MANUAL - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_LOITER - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_LOITER to MANUAL - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - not rotary",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
+			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
+			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: ALTCTL to MANUAL - local altitude valid",
+			MTT_LOC_ALT_VALID,
+			MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		// TRANSITION_DENIED tests
+
+		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
+			MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
+		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
+			MTT_ROTARY_WING,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
+	};
+	
+	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
+	for (size_t i=0; i<cMainTransitionTests; i++) {
+		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
+
+		// Setup initial machine state
+		struct vehicle_status_s current_state;
+		current_state.main_state = test->from_state;
+		current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
+		current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
+		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
+		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
+		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
+		
+		// Attempt transition
+		transition_result_t result = main_state_transition(&current_state, test->to_state);
+		
+		// Validate result of transition
+		ut_assert(test->assertMsg, test->expected_transition_result == result);
+		if (test->expected_transition_result == result) {
+			if (test->expected_transition_result == TRANSITION_CHANGED) {
+				ut_assert(test->assertMsg, test->to_state == current_state.main_state);
+			} else {
+				ut_assert(test->assertMsg, test->from_state == current_state.main_state);
+			}
+		}
+	}
+
+
+	return true;
 }
 
-const char*
-StateMachineHelperTest::is_safe_test()
+bool StateMachineHelperTest::isSafeTest(void)
 {
 	struct vehicle_status_s current_state;
 	struct safety_s safety;
@@ -199,49 +459,45 @@ StateMachineHelperTest::is_safe_test()
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = false;
 	armed.lockdown = true;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
+	ut_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
-	mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
+	ut_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
 
-	return 0;
+	return true;
 }
 
-const char*
-StateMachineHelperTest::run_tests()
+void StateMachineHelperTest::runTests(void)
 {
-	mu_run_test(arming_state_transition_test);
-	mu_run_test(arming_state_transition_arm_disarm_test);
-	mu_run_test(main_state_transition_test);
-	mu_run_test(is_safe_test);
-
-	return 0;
+	ut_run_test(armingStateTransitionTest);
+	ut_run_test(mainStateTransitionTest);
+	ut_run_test(isSafeTest);
 }
 
-void
-state_machine_helper_test()
+void stateMachineHelperTest(void)
 {
 	StateMachineHelperTest* test = new StateMachineHelperTest();
-	test->UnitTest::print_results(test->run_tests());
+    test->runTests();
+	test->printResults();
 }
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
index 10a68e602..bbf66255e 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.h
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -39,6 +39,6 @@
 #ifndef STATE_MACHINE_HELPER_TEST_H_
 #define STATE_MACHINE_HELPER_TEST_
 
-void state_machine_helper_test();
+void stateMachineHelperTest(void);
 
 #endif /* STATE_MACHINE_HELPER_TEST_H_ */
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 30cd0d48d..cbc2844c1 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd)
 				gyro_scale.z_offset += gyro_report.z;
 				calibration_counter++;
 
-				if (calibration_counter % (calibration_count / 20) == 0)
+				if (calibration_counter % (calibration_count / 20) == 0) {
 					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+				}
 
 			} else {
 				poll_errcount++;
@@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd)
 	/* apply new offsets */
 	fd = open(GYRO_DEVICE_PATH, 0);
 
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
 		warn("WARNING: failed to apply new offsets for gyro");
+	}
 
 	close(fd);
 
@@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd)
 
 	float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
 
-	if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
+	if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; }
 
-	if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
+	if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; }
 
 
 	uint64_t last_time = hrt_absolute_time();
@@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd)
 			//float mag = -atan2f(magNav(1),magNav(0));
 			float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
 
-			if (mag > M_PI_F) mag -= 2 * M_PI_F;
+			if (mag > M_PI_F) { mag -= 2 * M_PI_F; }
 
-			if (mag < -M_PI_F) mag += 2 * M_PI_F;
+			if (mag < -M_PI_F) { mag += 2 * M_PI_F; }
 
 			float diff = mag - mag_last;
 
-			if (diff > M_PI_F) diff -= 2 * M_PI_F;
+			if (diff > M_PI_F) { diff -= 2 * M_PI_F; }
 
-			if (diff < -M_PI_F) diff += 2 * M_PI_F;
+			if (diff < -M_PI_F) { diff += 2 * M_PI_F; }
 
 			baseline_integral += diff;
 			mag_last = mag;
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 4ebf266f4..0ead22f77 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd)
 	uint64_t calibration_interval = 45 * 1000 * 1000;
 
 	/* maximum 500 values */
-	const unsigned int calibration_maxcount = 500;
+	const unsigned int calibration_maxcount = 240;
 	unsigned int calibration_counter;
 
 	struct mag_scale mscale_null = {
@@ -121,9 +121,24 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (x == NULL || y == NULL || z == NULL) {
 			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
+
+			/* clean up */
+			if (x != NULL) {
+				free(x);
+			}
+
+			if (y != NULL) {
+				free(y);
+			}
+
+			if (z != NULL) {
+				free(z);
+			}
+
 			res = ERROR;
 			return res;
 		}
+
 	} else {
 		/* exit */
 		return ERROR;
@@ -163,8 +178,9 @@ int do_mag_calibration(int mavlink_fd)
 
 				calibration_counter++;
 
-				if (calibration_counter % (calibration_maxcount / 20) == 0)
+				if (calibration_counter % (calibration_maxcount / 20) == 0) {
 					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
+				}
 
 			} else {
 				poll_errcount++;
@@ -198,14 +214,17 @@ int do_mag_calibration(int mavlink_fd)
 		}
 	}
 
-	if (x != NULL)
+	if (x != NULL) {
 		free(x);
+	}
 
-	if (y != NULL)
+	if (y != NULL) {
 		free(y);
+	}
 
-	if (z != NULL)
+	if (z != NULL) {
 		free(z);
+	}
 
 	if (res == OK) {
 		/* apply calibration and set parameters */
@@ -234,23 +253,29 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (res == OK) {
 			/* set parameters */
-			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
+			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
+			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
+			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
+			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
+			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
+			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
 				res = ERROR;
+			}
 
 			if (res != OK) {
 				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h
index a101cd7b1..c14f948cc 100644
--- a/src/modules/commander/mag_calibration.h
+++ b/src/modules/commander/mag_calibration.h
@@ -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
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 554dfcb08..27ca5c182 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -47,3 +47,7 @@ SRCS		 	= commander.cpp \
 			baro_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp
+
+MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index b60a7e0b9..eaf309288 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -8,11 +8,15 @@
 #ifndef PX4_CUSTOM_MODE_H_
 #define PX4_CUSTOM_MODE_H_
 
+#include <stdint.h>
+
 enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
-	PX4_CUSTOM_MAIN_MODE_SEATBELT,
-	PX4_CUSTOM_MAIN_MODE_EASY,
+	PX4_CUSTOM_MAIN_MODE_ALTCTL,
+	PX4_CUSTOM_MAIN_MODE_POSCTL,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
+	PX4_CUSTOM_MAIN_MODE_ACRO,
+	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
@@ -22,6 +26,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/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index 41f3ca0aa..0776894fb 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd)
 	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
 
 	/* set parameters */
-	float p = sp.roll;
+	float p = sp.y;
 	param_set(param_find("TRIM_ROLL"), &p);
-	p = sp.pitch;
+	p = sp.x;
 	param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.yaw;
+	p = sp.r;
 	param_set(param_find("TRIM_YAW"), &p);
 
 	/* store to permanent storage */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 7ae61d9ef..372ba9d7d 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>
+ *   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
@@ -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>
@@ -44,15 +45,20 @@
 #include <stdbool.h>
 #include <dirent.h>
 #include <fcntl.h>
+#include <string.h>
+#include <math.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_airspeed.h>
 #include <drivers/drv_device.h>
 #include <mavlink/mavlink_log.h>
 
@@ -65,134 +71,157 @@
 #endif
 static const int ERROR = -1;
 
-static bool arming_state_changed = true;
-static bool main_state_changed = true;
-static bool navigation_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
+};
+
+// You can index into the array with an arming_state_t in order to get it's textual representation
+static const char * const state_names[ARMING_STATE_MAX] = {
+	"ARMING_STATE_INIT",
+	"ARMING_STATE_STANDBY",
+	"ARMING_STATE_ARMED",
+	"ARMING_STATE_ARMED_ERROR",
+	"ARMING_STATE_STANDBY_ERROR",
+	"ARMING_STATE_REBOOT",
+	"ARMING_STATE_IN_AIR_RESTORE",
+};
 
 transition_result_t
-arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
-	const struct vehicle_control_mode_s *control_mode,
-	arming_state_t new_arming_state, struct actuator_armed_s *armed)
+arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
+			const struct safety_s   *safety,            /// current safety settings
+			arming_state_t          new_arming_state,   /// arming state requested
+			struct actuator_armed_s *armed,             /// current armed status
+			const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
 {
-	/*
-	 * Perform an atomic state update
-	 */
-	irqstate_t flags = irqsave();
+	// Double check that our static arrays are still valid
+	ASSERT(ARMING_STATE_INIT == 0);
+	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
 	transition_result_t ret = TRANSITION_DENIED;
 
+	arming_state_t current_arming_state = status->arming_state;
+
 	/* only check transition if the new state is actually different from the current one */
-	if (new_arming_state == status->arming_state) {
+	if (new_arming_state == current_arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
 
+		/*
+		 * Get sensing state if necessary
+		 */
+		int prearm_ret = OK;
+
+		/* only perform the check if we have to */
+		if (new_arming_state == ARMING_STATE_ARMED) {
+			prearm_ret = prearm_check(status, mavlink_fd);
+		}
+
+		/*
+		 * Perform an atomic state update
+		 */
+		irqstate_t flags = irqsave();
+
 		/* enforce lockdown in HIL */
-		if (control_mode->flag_system_hil_enabled) {
+		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
+
 		} else {
 			armed->lockdown = false;
 		}
 
-		switch (new_arming_state) {
-		case ARMING_STATE_INIT:
+		// Check that we have a valid state transition
+		bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
 
-			/* allow going back from INIT for calibration */
-			if (status->arming_state == ARMING_STATE_STANDBY) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
+		if (valid_transition) {
+			// We have a good transition. Now perform any secondary validation.
+			if (new_arming_state == ARMING_STATE_ARMED) {
 
-			break;
+				//      Do not perform pre-arm checks if coming from in air restore
+				//      Allow if HIL_STATE_ON
+				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
+					status->hil_state == HIL_STATE_OFF) {
 
-		case ARMING_STATE_STANDBY:
+					// Fail transition if pre-arm check fails
+					if (prearm_ret) {
+						valid_transition = false;
 
-			/* allow coming from INIT and disarming from ARMED */
-			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED
-			    || control_mode->flag_system_hil_enabled) {
+					// Fail transition if we need safety switch press
+					} else if (safety->safety_switch_available && !safety->safety_off) {
 
-				/* sensors need to be initialized for STANDBY state */
-				if (status->condition_system_sensors_initialized) {
-					ret = TRANSITION_CHANGED;
-					armed->armed = false;
-					armed->ready_to_arm = true;
-				}
-			}
-
-			break;
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
 
-		case ARMING_STATE_ARMED:
-
-			/* allow arming from STANDBY and IN-AIR-RESTORE */
-			if ((status->arming_state == ARMING_STATE_STANDBY
-			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
-				ret = TRANSITION_CHANGED;
-				armed->armed = true;
-				armed->ready_to_arm = true;
-			}
-
-			break;
-
-		case ARMING_STATE_ARMED_ERROR:
-
-			/* an armed error happens when ARMED obviously */
-			if (status->arming_state == ARMING_STATE_ARMED) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = true;
-				armed->ready_to_arm = false;
-			}
+						valid_transition = false;
+					}
 
-			break;
+					// Perform power checks only if circuit breaker is not
+					// engaged for these checks
+					if (!status->circuit_breaker_engaged_power_check) {
+						// Fail transition if power is not good
+						if (!status->condition_power_input_valid) {
 
-		case ARMING_STATE_STANDBY_ERROR:
+							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							valid_transition = false;
+						}
 
-			/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
-			if (status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
+						// Fail transition if power levels on the avionics rail
+						// are measured but are insufficient
+						if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
+							(status->avionics_power_rail_voltage < 4.9f))) {
 
-			break;
+							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+							valid_transition = false;
+						}
+					}
 
-		case ARMING_STATE_REBOOT:
+				}
 
-			/* an armed error happens when ARMED obviously */
-			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
+			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
+				new_arming_state = ARMING_STATE_STANDBY_ERROR;
 			}
+		}
 
-			break;
-
-		case ARMING_STATE_IN_AIR_RESTORE:
-
-			/* XXX implement */
-			break;
+		// HIL can always go to standby
+		if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
+			valid_transition = true;
+		}
 
-		default:
-			break;
+		/* Sensors need to be initialized for STANDBY state */
+		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			valid_transition = false;
 		}
 
-		if (ret == TRANSITION_CHANGED) {
+		// Finish up the state transition
+		if (valid_transition) {
+			armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
+			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;
 		}
+
+		/* end of atomic state update */
+		irqrestore(flags);
 	}
 
-	/* end of atomic state update */
-	irqrestore(flags);
+	if (ret == TRANSITION_DENIED) {
+		static const char *errMsg = "INVAL: %s - %s";
 
-	if (ret == TRANSITION_DENIED)
-		warnx("arming transition rejected");
+		mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+
+		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+	}
 
 	return ret;
 }
@@ -211,528 +240,448 @@ 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 *current_state, main_state_t new_main_state)
+main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* only check transition if the new state is actually different from the current one */
-	if (new_main_state == current_state->main_state) {
-		ret = TRANSITION_NOT_CHANGED;
-
-	} else {
-
-		switch (new_main_state) {
-		case MAIN_STATE_MANUAL:
+	/* transition may be denied even if the same state is requested because conditions may have changed */
+	switch (new_main_state) {
+	case MAIN_STATE_MANUAL:
+	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_SEATBELT:
-
-			/* need at minimum altitude estimate */
-			if (current_state->condition_local_altitude_valid ||
-				current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
-
-			break;
-
-		case MAIN_STATE_EASY:
-
-			/* need at minimum local position estimate */
-			if (current_state->condition_local_position_valid ||
-				current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
-
-			break;
-
-		case MAIN_STATE_AUTO:
-
-			/* need global position estimate */
-			if (current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
-
-			break;
-		}
-
-		if (ret == TRANSITION_CHANGED) {
-			current_state->main_state = new_main_state;
-			main_state_changed = true;
 		}
-	}
-
-	return ret;
-}
+		break;
 
-bool
-check_main_state_changed()
-{
-	if (main_state_changed) {
-		main_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
-transition_result_t
-navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
-{
-	transition_result_t ret = TRANSITION_DENIED;
-
-	/* only check transition if the new state is actually different from the current one */
-	if (new_navigation_state == status->navigation_state) {
-		ret = TRANSITION_NOT_CHANGED;
-
-	} else {
-
-		switch (new_navigation_state) {
-		case NAVIGATION_STATE_DIRECT:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_STABILIZE:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_ALTHOLD:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_VECTOR:
+	case MAIN_STATE_POSCTL:
+		/* need at minimum local position estimate */
+		if (status->condition_local_position_valid ||
+		    status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_AUTO_READY:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = false;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
+		}
+		break;
 
-		case NAVIGATION_STATE_AUTO_TAKEOFF:
+	case MAIN_STATE_AUTO_LOITER:
+		/* need global position estimate */
+		if (status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
+		}
+		break;
 
-		case NAVIGATION_STATE_AUTO_LOITER:
+	case MAIN_STATE_AUTO_MISSION:
+	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;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = false;
-			break;
+		}
+		break;
 
-		case NAVIGATION_STATE_AUTO_MISSION:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
+	case MAIN_STATE_OFFBOARD:
 
-		case NAVIGATION_STATE_AUTO_RTL:
+		/* need offboard signal */
+		if (!status->offboard_control_signal_lost) {
 			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_LAND:
-
-			/* deny transitions from landed state */
-			if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
-				ret = TRANSITION_CHANGED;
-				control_mode->flag_control_rates_enabled = true;
-				control_mode->flag_control_attitude_enabled = true;
-				control_mode->flag_control_velocity_enabled = true;
-				control_mode->flag_control_position_enabled = true;
-				control_mode->flag_control_altitude_enabled = true;
-				control_mode->flag_control_climb_rate_enabled = true;
-				control_mode->flag_control_manual_enabled = false;
-				control_mode->flag_control_auto_enabled = true;
-			}
-
-			break;
-
-		default:
-			break;
 		}
 
-		if (ret == TRANSITION_CHANGED) {
-			status->navigation_state = new_navigation_state;
-			control_mode->auto_state = status->navigation_state;
-			navigation_state_changed = true;
+		break;
+
+	case MAIN_STATE_MAX:
+	default:
+		break;
+	}
+	if (ret == TRANSITION_CHANGED) {
+		if (status->main_state != new_main_state) {
+			status->main_state = new_main_state;
+		} else {
+			ret = TRANSITION_NOT_CHANGED;
 		}
 	}
 
 	return ret;
 }
 
-bool
-check_navigation_state_changed()
-{
-	if (navigation_state_changed) {
-		navigation_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
-void
-set_navigation_state_changed()
-{
-	navigation_state_changed = true;
-}
-
 /**
-* 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, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, 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;
-
-	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
+	transition_result_t ret = TRANSITION_DENIED;
 
 	if (current_status->hil_state == new_state) {
-		warnx("Hil state not changed");
-		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, "Not switching off HIL (safety)");
-			valid_transition = false;
-
+			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
+			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) {
 
-				current_control_mode->flag_system_hil_enabled = true;
-				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;
-				struct dirent	*direntry;
 				d = opendir("/dev");
+
 				if (d) {
+					struct dirent	*direntry;
+					char devname[24];
 
 					while ((direntry = readdir(d)) != NULL) {
 
-						int sensfd = ::open(direntry->d_name, 0);
-						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						/* skip serial ports */
+						if (!strncmp("tty", direntry->d_name, 3)) {
+							continue;
+						}
+
+						/* skip mtd devices */
+						if (!strncmp("mtd", direntry->d_name, 3)) {
+							continue;
+						}
+
+						/* skip ram devices */
+						if (!strncmp("ram", direntry->d_name, 3)) {
+							continue;
+						}
+
+						/* skip MMC devices */
+						if (!strncmp("mmc", direntry->d_name, 3)) {
+							continue;
+						}
+
+						/* skip mavlink */
+						if (!strcmp("mavlink", direntry->d_name)) {
+							continue;
+						}
+
+						/* skip console */
+						if (!strcmp("console", direntry->d_name)) {
+							continue;
+						}
+
+						/* skip null */
+						if (!strcmp("null", direntry->d_name)) {
+							continue;
+						}
+
+						snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
+
+						int sensfd = ::open(devname, 0);
+
+						if (sensfd < 0) {
+							warn("failed opening device %s", devname);
+							continue;
+						}
+
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
 						close(sensfd);
 
-						printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+						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");
 
-					warnx("directory listing ok (FS mounted and readable)");
 
 				} 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();
+		// XXX also set lockdown here
 		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+	}
+	return ret;
+}
 
-		current_control_mode->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
+/**
+ * Check failsafe and main status and set navigation status for navigator accordingly
+ */
+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;
+
+	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 */
+	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 && armed) {
+			status->failsafe = true;
+
+			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;
+			}
 
-		// XXX also set lockdown here
+		} else {
+			switch (status->main_state) {
+			case MAIN_STATE_ACRO:
+				status->nav_state = NAVIGATION_STATE_ACRO;
+				break;
+
+			case MAIN_STATE_MANUAL:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+
+			case MAIN_STATE_ALTCTL:
+				status->nav_state = NAVIGATION_STATE_ALTCTL;
+				break;
+
+			case MAIN_STATE_POSCTL:
+				status->nav_state = NAVIGATION_STATE_POSCTL;
+				break;
+
+			default:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+			}
+		}
+		break;
+
+	case MAIN_STATE_AUTO_MISSION:
+		/* 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;
+
+			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;
+			}
 
-		ret = OK;
+		/* 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_RTGS;
+			} 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 {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+		/* 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:
+		/* 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_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;
+			}
+
+		/* 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_RTGS;
+			} 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_RTGS;
+			} 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 {
+			/* 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)) {
+			status->failsafe = true;
+
+			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 {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+		}
+		break;
+
+	case MAIN_STATE_OFFBOARD:
+		/* require offboard control, otherwise stay where you are */
+		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
+			status->failsafe = true;
+
+			status->nav_state = NAVIGATION_STATE_POSCTL;
+		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
+			status->failsafe = true;
+
+			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 {
+			status->nav_state = NAVIGATION_STATE_OFFBOARD;
+		}
+	default:
+		break;
 	}
 
-	return ret;
+	return status->nav_state != nav_state_old;
 }
 
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+{
+	int ret;
+	bool failed = false;
+
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
+		failed = true;
+		goto system_eval;
+	}
+
+	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
+		failed = true;
+		goto system_eval;
+	}
+
+	/* check measurement result range */
+	struct accel_report acc;
+	ret = read(fd, &acc, sizeof(acc));
+
+	if (ret == sizeof(acc)) {
+		/* evaluate values */
+		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+
+		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
+			mavlink_log_critical(mavlink_fd, "hold still while arming");
+			/* this is frickin' fatal */
+			failed = true;
+			goto system_eval;
+		}
+	} else {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
+		/* this is frickin' fatal */
+		failed = true;
+		goto system_eval;
+	}
 
+	if (!status->is_rotary_wing) {
+		/* accel done, close it */
+		close(fd);
+		fd = orb_subscribe(ORB_ID(airspeed));
 
-// /*
-//  * 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;
-//}
+		struct airspeed_s airspeed;
 
+		if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
+			(hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) {
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
+			failed = true;
+			goto system_eval;
+		}
+
+		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
+			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
+			// XXX do not make this fatal yet
+		}
+	}
+
+system_eval:
+	close(fd);
+	return (failed);
+}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0bfdf36a8..bb1b87e71 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -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>
+ *   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
@@ -36,19 +34,18 @@
 /**
  * @file state_machine_helper.h
  * State machine helper functions definitions
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef STATE_MACHINE_HELPER_H_
 #define STATE_MACHINE_HELPER_H_
 
-#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
-#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
-#include <uORB/topics/vehicle_control_mode.h>
 
 typedef enum {
 	TRANSITION_DENIED = -1,
@@ -57,23 +54,17 @@ typedef enum {
 
 } transition_result_t;
 
-transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
-
 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 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);
 
 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 navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
-
-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);
 
-void set_navigation_state_changed();
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
 
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index b964d40a3..6768bfa7e 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -41,10 +41,11 @@
 #include <string.h>
 #include <stdio.h>
 
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+
 #include "Block.hpp"
 #include "BlockParam.hpp"
-#include "../uorb/UOrbSubscription.hpp"
-#include "../uorb/UOrbPublication.hpp"
 
 namespace control
 {
@@ -100,7 +101,7 @@ void Block::updateParams()
 
 void Block::updateSubscriptions()
 {
-	UOrbSubscriptionBase *sub = getSubscriptions().getHead();
+	uORB::SubscriptionBase *sub = getSubscriptions().getHead();
 	int count = 0;
 
 	while (sub != NULL) {
@@ -118,7 +119,7 @@ void Block::updateSubscriptions()
 
 void Block::updatePublications()
 {
-	UOrbPublicationBase *pub = getPublications().getHead();
+	uORB::PublicationBase *pub = getPublications().getHead();
 	int count = 0;
 
 	while (pub != NULL) {
diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index 258701f27..736698e21 100644
--- a/src/modules/controllib/block/Block.hpp
+++ b/src/modules/controllib/block/Block.hpp
@@ -42,7 +42,13 @@
 #include <stdint.h>
 #include <inttypes.h>
 
-#include "List.hpp"
+#include <containers/List.hpp>
+
+// forward declaration
+namespace uORB {
+	class SubscriptionBase;
+	class PublicationBase;
+}
 
 namespace control
 {
@@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80;
 
 // forward declaration
 class BlockParamBase;
-class UOrbSubscriptionBase;
-class UOrbPublicationBase;
 class SuperBlock;
 
 /**
@@ -79,15 +83,15 @@ public:
 protected:
 // accessors
 	SuperBlock *getParent() { return _parent; }
-	List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
-	List<UOrbPublicationBase *> & getPublications() { return _publications; }
+	List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
+	List<uORB::PublicationBase *> & getPublications() { return _publications; }
 	List<BlockParamBase *> & getParams() { return _params; }
 // attributes
 	const char *_name;
 	SuperBlock *_parent;
 	float _dt;
-	List<UOrbSubscriptionBase *> _subscriptions;
-	List<UOrbPublicationBase *> _publications;
+	List<uORB::SubscriptionBase *> _subscriptions;
+	List<uORB::PublicationBase *> _publications;
 	List<BlockParamBase *> _params;
 };
 
diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp
index fd12e365d..8f98da74f 100644
--- a/src/modules/controllib/block/BlockParam.cpp
+++ b/src/modules/controllib/block/BlockParam.cpp
@@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
 		printf("error finding param: %s\n", fullname);
 };
 
+template <class T>
+BlockParam<T>::BlockParam(Block *block, const char *name,
+		bool parent_prefix) :
+	BlockParamBase(block, name, parent_prefix),
+	_val() {
+	update();
+}
+
+template <class T>
+T BlockParam<T>::get() { return _val; }
+
+template <class T>
+void BlockParam<T>::set(T val) { _val = val; }
+
+template <class T>
+void BlockParam<T>::update() {
+	if (_handle != PARAM_INVALID) param_get(_handle, &_val);
+}
+
+template <class T>
+BlockParam<T>::~BlockParam() {};
+
+template class __EXPORT BlockParam<float>;
+template class __EXPORT BlockParam<int>;
+
 } // namespace control
diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp
index 36bc8c24b..a64d0139e 100644
--- a/src/modules/controllib/block/BlockParam.hpp
+++ b/src/modules/controllib/block/BlockParam.hpp
@@ -42,7 +42,7 @@
 #include <systemlib/param/param.h>
 
 #include "Block.hpp"
-#include "List.hpp"
+#include <containers/List.hpp>
 
 namespace control
 {
@@ -70,38 +70,21 @@ protected:
  * Parameters that are tied to blocks for updating and nameing.
  */
 
-class __EXPORT BlockParamFloat : public BlockParamBase
+template <class T>
+class BlockParam : public BlockParamBase
 {
 public:
-	BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) :
-		BlockParamBase(block, name, parent_prefix),
-		_val() {
-		update();
-	}
-	float get() { return _val; }
-	void set(float val) { _val = val; }
-	void update() {
-		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
-	}
+	BlockParam(Block *block, const char *name,
+			bool parent_prefix=true);
+	T get();
+	void set(T val);
+	void update();
+	virtual ~BlockParam();
 protected:
-	float _val;
+	T _val;
 };
 
-class __EXPORT BlockParamInt : public BlockParamBase
-{
-public:
-	BlockParamInt(Block *block, const char *name, bool parent_prefix=true) :
-		BlockParamBase(block, name, parent_prefix),
-		_val() {
-		update();
-	}
-	int get() { return _val; }
-	void set(int val) { _val = val; }
-	void update() {
-		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
-	}
-protected:
-	int _val;
-};
+typedef BlockParam<float> BlockParamFloat;
+typedef BlockParam<int> BlockParamInt;
 
 } // namespace control
diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index d815a8feb..f0139a4b7 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -37,7 +37,5 @@
 SRCS		 = test_params.c \
 		   block/Block.cpp \
 		   block/BlockParam.cpp \
-		   uorb/UOrbPublication.cpp \
-		   uorb/UOrbSubscription.cpp \
 		   uorb/blocks.cpp \
 		   blocks.cpp
diff --git a/src/modules/controllib/uorb/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp
deleted file mode 100644
index f69b39d90..000000000
--- a/src/modules/controllib/uorb/UOrbPublication.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 UOrbPublication.cpp
- *
- */
-
-#include "UOrbPublication.hpp"
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
index 448a42a99..e8fecef0d 100644
--- a/src/modules/controllib/uorb/blocks.cpp
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
 
 void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
 				   vehicle_attitude_s &att,
-				   vehicle_global_position_setpoint_s &posCmd,
-				   vehicle_global_position_setpoint_s &lastPosCmd)
+				   position_setpoint_s &missionCmd,
+				   position_setpoint_s &lastMissionCmd)
 {
 
 	// heading to waypoint
 	float psiTrack = get_bearing_to_next_waypoint(
 				 (double)pos.lat / (double)1e7d,
 				 (double)pos.lon / (double)1e7d,
-				 (double)posCmd.lat / (double)1e7d,
-				 (double)posCmd.lon / (double)1e7d);
+				 missionCmd.lat,
+				 missionCmd.lon);
 
 	// cross track
 	struct crosstrack_error_s xtrackError;
 	get_distance_to_line(&xtrackError,
 			     (double)pos.lat / (double)1e7d,
 			     (double)pos.lon / (double)1e7d,
-			     (double)lastPosCmd.lat / (double)1e7d,
-			     (double)lastPosCmd.lon / (double)1e7d,
-			     (double)posCmd.lat / (double)1e7d,
-			     (double)posCmd.lon / (double)1e7d);
+			     lastMissionCmd.lat,
+			     lastMissionCmd.lon,
+			     missionCmd.lat,
+			     missionCmd.lon);
 
 	_psiCmd = _wrap_2pi(psiTrack -
 			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
@@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
 	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
 	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
 	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
+	_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
 	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
 	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 46dc1bec2..a8a70507e 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -43,7 +43,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
@@ -62,8 +62,8 @@ extern "C" {
 }
 
 #include "../blocks.hpp"
-#include "UOrbSubscription.hpp"
-#include "UOrbPublication.hpp"
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 
 namespace control
 {
@@ -82,8 +82,8 @@ public:
 	virtual ~BlockWaypointGuidance();
 	void update(vehicle_global_position_s &pos,
 		    vehicle_attitude_s &att,
-		    vehicle_global_position_setpoint_s &posCmd,
-		    vehicle_global_position_setpoint_s &lastPosCmd);
+		    position_setpoint_s &missionCmd,
+		    position_setpoint_s &lastMissionCmd);
 	float getPsiCmd() { return _psiCmd; }
 };
 
@@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
 {
 protected:
 	// subscriptions
-	UOrbSubscription<vehicle_attitude_s> _att;
-	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
-	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
-	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
-	UOrbSubscription<manual_control_setpoint_s> _manual;
-	UOrbSubscription<vehicle_status_s> _status;
-	UOrbSubscription<parameter_update_s> _param_update;
+	uORB::Subscription<vehicle_attitude_s> _att;
+	uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
+	uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
+	uORB::Subscription<vehicle_global_position_s> _pos;
+	uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
+	uORB::Subscription<manual_control_setpoint_s> _manual;
+	uORB::Subscription<vehicle_status_s> _status;
+	uORB::Subscription<parameter_update_s> _param_update;
 	// publications
-	UOrbPublication<actuator_controls_s> _actuators;
+	uORB::Publication<actuator_controls_s> _actuators;
 public:
 	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
 	virtual ~BlockUorbEnabledAutopilot();
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
new file mode 100644
index 000000000..ca1fe9bbb
--- /dev/null
+++ b/src/modules/dataman/dataman.c
@@ -0,0 +1,872 @@
+/****************************************************************************
+ *
+ *   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 dataman.c
+ * DATAMANAGER driver.
+ *
+ * @author Jean Cyr
+ * @author Lorenz Meier
+ * @author Julian Oes
+ * @author Thomas Gubler
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <queue.h>
+#include <string.h>
+
+#include "dataman.h"
+#include <systemlib/param/param.h>
+
+/**
+ * data manager app start / stop handling function
+ *
+ * @ingroup apps
+ */
+
+__EXPORT int dataman_main(int argc, char *argv[]);
+__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
+__EXPORT ssize_t dm_write(dm_item_t  item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
+__EXPORT int dm_clear(dm_item_t item);
+__EXPORT void dm_lock(dm_item_t item);
+__EXPORT void dm_unlock(dm_item_t item);
+__EXPORT int dm_restart(dm_reset_reason restart_type);
+
+/** Types of function calls supported by the worker task */
+typedef enum {
+	dm_write_func = 0,
+	dm_read_func,
+	dm_clear_func,
+	dm_restart_func,
+	dm_number_of_funcs
+} dm_function_t;
+
+/** Work task work item */
+typedef struct {
+	sq_entry_t link;	/**< list linkage */
+	sem_t wait_sem;
+	unsigned char first;
+	unsigned char func;
+	ssize_t result;
+	union {
+		struct {
+			dm_item_t item;
+			unsigned char index;
+			dm_persitence_t persistence;
+			const void *buf;
+			size_t count;
+		} write_params;
+		struct {
+			dm_item_t item;
+			unsigned char index;
+			void *buf;
+			size_t count;
+		} read_params;
+		struct {
+			dm_item_t item;
+		} clear_params;
+		struct {
+			dm_reset_reason reason;
+		} restart_params;
+	};
+} work_q_item_t;
+
+const size_t k_work_item_allocation_chunk_size = 8;
+
+/* Usage statistics */
+static unsigned g_func_counts[dm_number_of_funcs];
+
+/* table of maximum number of instances for each item type */
+static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
+	DM_KEY_SAFE_POINTS_MAX,
+	DM_KEY_FENCE_POINTS_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
+	DM_KEY_WAYPOINTS_ONBOARD_MAX,
+	DM_KEY_MISSION_STATE_MAX
+};
+
+/* Table of offset for index 0 of each item type */
+static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
+
+/* Item type lock mutexes */
+static sem_t *g_item_locks[DM_KEY_NUM_KEYS];
+static sem_t g_sys_state_mutex;
+
+/* The data manager store file handle and file name */
+static int g_fd = -1, g_task_fd = -1;
+static const char *k_data_manager_device_path = "/fs/microsd/dataman";
+
+/* The data manager work queues */
+
+typedef struct {
+	sq_queue_t q;		/* Nuttx queue */
+	sem_t mutex;		/* Mutual exclusion on work queue adds and deletes */
+	unsigned size;		/* Current size of queue */
+	unsigned max_size;	/* Maximum queue size reached */
+} work_q_t;
+
+static work_q_t g_free_q;	/* queue of free work items. So that we don't always need to call malloc and free*/
+static work_q_t g_work_q;	/* pending work items. To be consumed by worker thread */
+
+sem_t g_work_queued_sema;	/* To notify worker thread a work item has been queued */
+sem_t g_init_sema;
+
+static bool g_task_should_exit;	/**< if true, dataman task should exit */
+
+#define DM_SECTOR_HDR_SIZE 4	/* data manager per item header overhead */
+static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
+
+static void init_q(work_q_t *q)
+{
+	sq_init(&(q->q));		/* Initialize the NuttX queue structure */
+	sem_init(&(q->mutex), 1, 1);	/* Queue is initially unlocked */
+	q->size = q->max_size = 0;	/* Queue is initially empty */
+}
+
+static inline void
+destroy_q(work_q_t *q)
+{
+	sem_destroy(&(q->mutex));	/* Destroy the queue lock */
+}
+
+static inline void
+lock_queue(work_q_t *q)
+{
+	sem_wait(&(q->mutex));	/* Acquire the queue lock */
+}
+
+static inline void
+unlock_queue(work_q_t *q)
+{
+	sem_post(&(q->mutex));	/* Release the queue lock */
+}
+
+static work_q_item_t *
+create_work_item(void)
+{
+	work_q_item_t *item;
+
+	/* Try to reuse item from free item queue */
+	lock_queue(&g_free_q);
+
+	if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
+		g_free_q.size--;
+
+	unlock_queue(&g_free_q);
+
+	/* If we there weren't any free items then obtain memory for a new ones */
+	if (item == NULL) {
+		item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t));
+		if (item) {
+			item->first = 1;
+			lock_queue(&g_free_q);
+			for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) {
+				(item + i)->first = 0;
+				sq_addfirst(&(item + i)->link, &(g_free_q.q));
+			}
+			/* Update the queue size and potentially the maximum queue size */
+			g_free_q.size += k_work_item_allocation_chunk_size - 1;
+			if (g_free_q.size > g_free_q.max_size)
+				g_free_q.max_size = g_free_q.size;
+			unlock_queue(&g_free_q);
+		}
+	}
+
+	/* If we got one then lock the item*/
+	if (item)
+		sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */
+
+	/* return the item pointer, or NULL if all failed */
+	return item;
+}
+
+/* Work queue management functions */
+
+static inline void
+destroy_work_item(work_q_item_t *item)
+{
+	sem_destroy(&item->wait_sem); /* Destroy the item lock */
+	/* Return the item to the free item queue for later reuse */
+	lock_queue(&g_free_q);
+	sq_addfirst(&item->link, &(g_free_q.q));
+
+	/* Update the queue size and potentially the maximum queue size */
+	if (++g_free_q.size > g_free_q.max_size)
+		g_free_q.max_size = g_free_q.size;
+
+	unlock_queue(&g_free_q);
+}
+
+static inline work_q_item_t *
+dequeue_work_item(void)
+{
+	work_q_item_t *work;
+
+	/* retrieve the 1st item on the work queue */
+	lock_queue(&g_work_q);
+
+	if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q)))
+		g_work_q.size--;
+
+	unlock_queue(&g_work_q);
+	return work;
+}
+
+static int
+enqueue_work_item_and_wait_for_result(work_q_item_t *item)
+{
+	/* put the work item at the end of the work queue */
+	lock_queue(&g_work_q);
+	sq_addlast(&item->link, &(g_work_q.q));
+
+	/* Adjust the queue size and potentially the maximum queue size */
+	if (++g_work_q.size > g_work_q.max_size)
+		g_work_q.max_size = g_work_q.size;
+
+	unlock_queue(&g_work_q);
+
+	/* tell the work thread that work is available */
+	sem_post(&g_work_queued_sema);
+
+	/* wait for the result */
+	sem_wait(&item->wait_sem);
+
+	int result = item->result;
+
+	destroy_work_item(item);
+
+	return result;
+}
+
+/* Calculate the offset in file of specific item */
+static int
+calculate_offset(dm_item_t item, unsigned char index)
+{
+
+	/* Make sure the item type is valid */
+	if (item >= DM_KEY_NUM_KEYS)
+		return -1;
+
+	/* Make sure the index for this item type is valid */
+	if (index >= g_per_item_max_index[item])
+		return -1;
+
+	/* Calculate and return the item index based on type and index */
+	return g_key_offsets[item] + (index * k_sector_size);
+}
+
+/* Each data item is stored as follows
+ *
+ * byte 0: Length of user data item
+ * byte 1: Persistence of this data item
+ * byte 2: Unused (for future use)
+ * byte 3: Unused (for future use)
+ * byte DM_SECTOR_HDR_SIZE... : data item value
+ *
+ * The total size must not exceed k_sector_size
+ */
+
+/* write to the data manager file */
+static ssize_t
+_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
+{
+	unsigned char buffer[k_sector_size];
+	size_t len;
+	int offset;
+
+	/* Get the offset for this item */
+	offset = calculate_offset(item, index);
+
+	/* If item type or index out of range, return error */
+	if (offset < 0)
+		return -1;
+
+	/* Make sure caller has not given us more data than we can handle */
+	if (count > DM_MAX_DATA_SIZE)
+		return -1;
+
+	/* Write out the data, prefixed with length and persistence level */
+	buffer[0] = count;
+	buffer[1] = persistence;
+	buffer[2] = 0;
+	buffer[3] = 0;
+	if (count > 0) {
+		memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+	}
+	count += DM_SECTOR_HDR_SIZE;
+
+	len = -1;
+
+	/* Seek to the right spot in the data manager file and write the data item */
+	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
+		if ((len = write(g_task_fd, buffer, count)) == count)
+			fsync(g_task_fd); /* Make sure data is written to physical media */
+
+	/* Make sure the write succeeded */
+	if (len != count)
+		return -1;
+
+	/* All is well... return the number of user data written */
+	return count - DM_SECTOR_HDR_SIZE;
+}
+
+/* Retrieve from the data manager file */
+static ssize_t
+_read(dm_item_t item, unsigned char index, void *buf, size_t count)
+{
+	unsigned char buffer[k_sector_size];
+	int len, offset;
+
+	/* Get the offset for this item */
+	offset = calculate_offset(item, index);
+
+	/* If item type or index out of range, return error */
+	if (offset < 0)
+		return -1;
+
+	/* Make sure the caller hasn't asked for more data than we can handle */
+	if (count > DM_MAX_DATA_SIZE)
+		return -1;
+
+	/* Read the prefix and data */
+	len = -1;
+
+	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
+		len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
+
+	/* Check for read error */
+	if (len < 0)
+		return -1;
+
+	/* A zero length entry is a empty entry */
+	if (len == 0)
+		buffer[0] = 0;
+
+	/* See if we got data */
+	if (buffer[0] > 0) {
+		/* We got more than requested!!! */
+		if (buffer[0] > count)
+			return -1;
+
+		/* Looks good, copy it to the caller's buffer */
+		memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]);
+	}
+
+	/* Return the number of bytes of caller data read */
+	return buffer[0];
+}
+
+static int
+_clear(dm_item_t item)
+{
+	int i, result = 0;
+
+	/* Get the offset of 1st item of this type */
+	int offset = calculate_offset(item, 0);
+
+	/* Check for item type out of range */
+	if (offset < 0)
+		return -1;
+
+	/* Clear all items of this type */
+	for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
+		char buf[1];
+
+		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+			result = -1;
+			break;
+		}
+
+		/* Avoid SD flash wear by only doing writes where necessary */
+		if (read(g_task_fd, buf, 1) < 1)
+			break;
+
+		/* If item has length greater than 0 it needs to be overwritten */
+		if (buf[0]) {
+			if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+				result = -1;
+				break;
+			}
+
+			buf[0] = 0;
+
+			if (write(g_task_fd, buf, 1) != 1) {
+				result = -1;
+				break;
+			}
+		}
+
+		offset += k_sector_size;
+	}
+
+	/* Make sure data is actually written to physical media */
+	fsync(g_task_fd);
+	return result;
+}
+
+/** Tell the data manager about the type of the last reset */
+static int
+_restart(dm_reset_reason reason)
+{
+	unsigned char buffer[2];
+	int offset = 0, result = 0;
+
+	/* We need to scan the entire file and invalidate and data that should not persist after the last reset */
+
+	/* Loop through all of the data segments and delete those that are not persistent */
+	while (1) {
+		size_t len;
+
+		/* Get data segment at current offset */
+		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+			/* must be at eof */
+			break;
+		}
+
+		len = read(g_task_fd, buffer, sizeof(buffer));
+
+		if (len != sizeof(buffer)) {
+			/* must be at eof */
+			break;
+		}
+
+		/* check if segment contains data */
+		if (buffer[0]) {
+			int clear_entry = 0;
+
+			/* Whether data gets deleted depends on reset type and data segment's persistence setting */
+			if (reason == DM_INIT_REASON_POWER_ON) {
+				if (buffer[1] > DM_PERSIST_POWER_ON_RESET) {
+					clear_entry = 1;
+				}
+
+			} else {
+				if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) {
+					clear_entry = 1;
+				}
+			}
+
+			/* Set segment to unused if data does not persist */
+			if (clear_entry) {
+				if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+					result = -1;
+					break;
+				}
+
+				buffer[0] = 0;
+
+				len = write(g_task_fd, buffer, 1);
+
+				if (len != 1) {
+					result = -1;
+					break;
+				}
+			}
+		}
+
+		offset += k_sector_size;
+	}
+
+	fsync(g_task_fd);
+
+	/* tell the caller how it went */
+	return result;
+}
+
+/** Write to the data manager file */
+__EXPORT ssize_t
+dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
+{
+	work_q_item_t *work;
+
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* get a work item and queue up a write request */
+	if ((work = create_work_item()) == NULL)
+		return -1;
+
+	work->func = dm_write_func;
+	work->write_params.item = item;
+	work->write_params.index = index;
+	work->write_params.persistence = persistence;
+	work->write_params.buf = buf;
+	work->write_params.count = count;
+
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
+}
+
+/** Retrieve from the data manager file */
+__EXPORT ssize_t
+dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
+{
+	work_q_item_t *work;
+
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* get a work item and queue up a read request */
+	if ((work = create_work_item()) == NULL)
+		return -1;
+
+	work->func = dm_read_func;
+	work->read_params.item = item;
+	work->read_params.index = index;
+	work->read_params.buf = buf;
+	work->read_params.count = count;
+
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
+}
+
+__EXPORT int
+dm_clear(dm_item_t item)
+{
+	work_q_item_t *work;
+
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* get a work item and queue up a clear request */
+	if ((work = create_work_item()) == NULL)
+		return -1;
+
+	work->func = dm_clear_func;
+	work->clear_params.item = item;
+
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return enqueue_work_item_and_wait_for_result(work);
+}
+
+__EXPORT void
+dm_lock(dm_item_t item)
+{
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return;
+	if (item >= DM_KEY_NUM_KEYS)
+		return;
+	if (g_item_locks[item]) {
+		sem_wait(g_item_locks[item]);
+	}
+}
+
+__EXPORT void
+dm_unlock(dm_item_t item)
+{
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return;
+	if (item >= DM_KEY_NUM_KEYS)
+		return;
+	if (g_item_locks[item]) {
+		sem_post(g_item_locks[item]);
+	}
+}
+
+/* Tell the data manager about the type of the last reset */
+__EXPORT int
+dm_restart(dm_reset_reason reason)
+{
+	work_q_item_t *work;
+
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* get a work item and queue up a restart request */
+	if ((work = create_work_item()) == NULL)
+		return -1;
+
+	work->func = dm_restart_func;
+	work->restart_params.reason = reason;
+
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return enqueue_work_item_and_wait_for_result(work);
+}
+
+static int
+task_main(int argc, char *argv[])
+{
+	work_q_item_t *work;
+
+	/* inform about start */
+	warnx("Initializing..");
+
+	/* Initialize global variables */
+	g_key_offsets[0] = 0;
+
+	for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++)
+		g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);
+
+	unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);
+
+	for (unsigned i = 0; i < dm_number_of_funcs; i++)
+		g_func_counts[i] = 0;
+
+	/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
+	sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
+	for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++)
+		g_item_locks[i] = NULL;
+	g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
+
+	g_task_should_exit = false;
+
+	init_q(&g_work_q);
+	init_q(&g_free_q);
+
+	sem_init(&g_work_queued_sema, 1, 0);
+
+	/* See if the data manage file exists and is a multiple of the sector size */
+	g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
+	if (g_task_fd >= 0) {
+		/* File exists, check its size */
+		int file_size = lseek(g_task_fd, 0, SEEK_END);
+		if ((file_size % k_sector_size) != 0) {
+			warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
+			close(g_task_fd);
+			unlink(k_data_manager_device_path);
+		}
+		else
+			close(g_task_fd);
+	}
+
+	/* Open or create the data manager file */
+	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
+
+	if (g_task_fd < 0) {
+		warnx("Could not open data manager file %s", k_data_manager_device_path);
+		sem_post(&g_init_sema); /* Don't want to hang startup */
+		return -1;
+	}
+
+	if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
+		close(g_task_fd);
+		warnx("Could not seek data manager file %s", k_data_manager_device_path);
+		sem_post(&g_init_sema); /* Don't want to hang startup */
+		return -1;
+	}
+
+	fsync(g_task_fd);
+
+	/* see if we need to erase any items based on restart type */
+	int sys_restart_val;
+	if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
+		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
+			warnx("Power on restart");
+			_restart(DM_INIT_REASON_POWER_ON);
+		}
+		else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
+			warnx("In flight restart");
+			_restart(DM_INIT_REASON_IN_FLIGHT);
+		}
+		else
+			warnx("Unknown restart");
+	}
+	else
+		warnx("Unknown restart");
+
+	/* We use two file descriptors, one for the caller context and one for the worker thread */
+	/* They are actually the same but we need to some way to reject caller request while the */
+	/* worker thread is shutting down but still processing requests */
+	g_fd = g_task_fd;
+
+	warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
+
+	/* Tell startup that the worker thread has completed its initialization */
+	sem_post(&g_init_sema);
+
+	/* Start the endless loop, waiting for then processing work requests */
+	while (true) {
+
+		/* do we need to exit ??? */
+		if ((g_task_should_exit) && (g_fd >= 0)) {
+			/* Close the file handle to stop further queuing */
+			g_fd = -1;
+		}
+
+		if (!g_task_should_exit) {
+			/* wait for work */
+			sem_wait(&g_work_queued_sema);
+		}
+
+		/* Empty the work queue */
+		while ((work = dequeue_work_item())) {
+
+			/* handle each work item with the appropriate handler */
+			switch (work->func) {
+			case dm_write_func:
+				g_func_counts[dm_write_func]++;
+				work->result =
+					_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count);
+				break;
+
+			case dm_read_func:
+				g_func_counts[dm_read_func]++;
+				work->result =
+					_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
+				break;
+
+			case dm_clear_func:
+				g_func_counts[dm_clear_func]++;
+				work->result = _clear(work->clear_params.item);
+				break;
+
+			case dm_restart_func:
+				g_func_counts[dm_restart_func]++;
+				work->result = _restart(work->restart_params.reason);
+				break;
+
+			default: /* should never happen */
+				work->result = -1;
+				break;
+			}
+
+			/* Inform the caller that work is done */
+			sem_post(&work->wait_sem);
+		}
+
+		/* time to go???? */
+		if ((g_task_should_exit) && (g_fd < 0))
+			break;
+	}
+
+	close(g_task_fd);
+	g_task_fd = -1;
+
+	/* The work queue is now empty, empty the free queue */
+	for (;;) {
+		if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
+			break;
+		if (work->first)
+			free(work);
+	}
+
+	destroy_q(&g_work_q);
+	destroy_q(&g_free_q);
+	sem_destroy(&g_work_queued_sema);
+	sem_destroy(&g_sys_state_mutex);
+
+	return 0;
+}
+
+static int
+start(void)
+{
+	int task;
+
+	sem_init(&g_init_sema, 1, 0);
+
+	/* start the worker thread */
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
+		warn("task start failed");
+		return -1;
+	}
+
+	/* wait for the thread to actually initialize */
+	sem_wait(&g_init_sema);
+	sem_destroy(&g_init_sema);
+
+	return 0;
+}
+
+static void
+status(void)
+{
+	/* display usage statistics */
+	warnx("Writes   %d", g_func_counts[dm_write_func]);
+	warnx("Reads    %d", g_func_counts[dm_read_func]);
+	warnx("Clears   %d", g_func_counts[dm_clear_func]);
+	warnx("Restarts %d", g_func_counts[dm_restart_func]);
+	warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size);
+}
+
+static void
+stop(void)
+{
+	/* Tell the worker task to shut down */
+	g_task_should_exit = true;
+	sem_post(&g_work_queued_sema);
+}
+
+static void
+usage(void)
+{
+	errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
+}
+
+int
+dataman_main(int argc, char *argv[])
+{
+	if (argc < 2)
+		usage();
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_fd >= 0)
+			errx(1, "already running");
+
+		start();
+
+		if (g_fd < 0)
+			errx(1, "start failed");
+
+		exit(0);
+	}
+
+	/* Worker thread should be running for all other commands */
+	if (g_fd < 0)
+		errx(1, "not running");
+
+	if (!strcmp(argv[1], "stop"))
+		stop();
+	else if (!strcmp(argv[1], "status"))
+		status();
+	else if (!strcmp(argv[1], "poweronrestart"))
+		dm_restart(DM_INIT_REASON_POWER_ON);
+	else if (!strcmp(argv[1], "inflightrestart"))
+		dm_restart(DM_INIT_REASON_IN_FLIGHT);
+	else
+		usage();
+
+	exit(1);
+}
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
new file mode 100644
index 000000000..d625bf985
--- /dev/null
+++ b/src/modules/dataman/dataman.h
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   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 dataman.h
+ *
+ * DATAMANAGER driver.
+ */
+#ifndef _DATAMANAGER_H
+#define _DATAMANAGER_H
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/fence.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/** Types of items that the data manager can store */
+	typedef enum {
+		DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
+		DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
+		DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
+		DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alernate between 0 and 1) */
+		DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
+		DM_KEY_MISSION_STATE,		/* Persistent mission state */
+		DM_KEY_NUM_KEYS			/* Total number of item types defined */
+	} dm_item_t;
+
+	#define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1)
+
+	/** The maximum number of instances for each item type */
+	enum {
+		DM_KEY_SAFE_POINTS_MAX = 8,
+		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
+		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_MISSION_STATE_MAX = 1
+	};
+
+	/** Data persistence levels */
+	typedef enum {
+		DM_PERSIST_POWER_ON_RESET = 0,	/* Data survives all resets */
+		DM_PERSIST_IN_FLIGHT_RESET,     /* Data survives in-flight resets only */
+		DM_PERSIST_VOLATILE             /* Data does not survive resets */
+	} dm_persitence_t;
+
+	/** The reason for the last reset */
+	typedef enum {
+		DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
+		DM_INIT_REASON_IN_FLIGHT,		/* Data survives in-flight resets only */
+		DM_INIT_REASON_VOLATILE			/* Data does not survive reset */
+	} dm_reset_reason;
+
+	/** Maximum size in bytes of a single item instance */
+	#define DM_MAX_DATA_SIZE 124
+
+	/** Retrieve from the data manager store */
+	__EXPORT ssize_t
+	dm_read(
+		dm_item_t item,			/* The item type to retrieve */
+		unsigned char index,		/* The index of the item */
+		void *buffer,			/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	);
+
+	/** write to the data manager store */
+	__EXPORT ssize_t
+	dm_write(
+		dm_item_t  item,		/* The item type to store */
+		unsigned char index,		/* The index of the item */
+		dm_persitence_t persistence,	/* The persistence level of this item */
+		const void *buffer,		/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	);
+
+	/** Lock all items of this type */
+	__EXPORT void
+	dm_lock(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/** Unlock all items of this type */
+	__EXPORT void
+	dm_unlock(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/** Erase all items of this type */
+	__EXPORT int
+	dm_clear(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/** Tell the data manager about the type of the last reset */
+	__EXPORT int
+	dm_restart(
+		dm_reset_reason restart_type	/* The last reset type */
+	);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk
new file mode 100644
index 000000000..234607b3d
--- /dev/null
+++ b/src/modules/dataman/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Main Navigation Controller
+#
+
+MODULE_COMMAND	= dataman
+
+SRCS		= dataman.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
new file mode 100644
index 000000000..40cb6043f
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -0,0 +1,1728 @@
+/****************************************************************************
+ *
+ *   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 ekf_att_pos_estimator_main.cpp
+ * Implementation of the attitude and position estimator.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <float.h>
+
+#define SENSOR_COMBINED_SUB
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+#ifdef SENSOR_COMBINED_SUB
+#include <uORB/topics/sensor_combined.h>
+#endif
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/wind_estimate.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+
+#include "estimator_23states.h"
+
+
+
+/**
+ * estimator app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
+
+__EXPORT uint32_t millis();
+
+static uint64_t IMUmsec = 0;
+static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
+
+uint32_t millis()
+{
+	return IMUmsec;
+}
+
+class FixedwingEstimator
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingEstimator();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~FixedwingEstimator();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return	OK on success.
+	 */
+	int		start();
+
+	/**
+	 * Task status
+	 *
+	 * @return	true if the mainloop is running
+	 */
+	bool		task_running() { return _task_running; }
+
+	/**
+	 * Print the current status.
+	 */
+	void		print_status();
+
+	/**
+	 * Trip the filter by feeding it NaN values.
+	 */
+	int		trip_nan();
+
+	/**
+	 * Enable logging.
+	 *
+	 * @param	enable Set to true to enable logging, false to disable
+	 */
+	int		enable_logging(bool enable);
+
+	/**
+	 * Set debug level.
+	 *
+	 * @param	debug Desired debug level - 0 to disable.
+	 */
+	int		set_debuglevel(unsigned debug) { _debug = debug; return 0; }
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_running;			/**< if true, task is running in its mainloop */
+	int		_estimator_task;		/**< task handle for sensor task */
+#ifndef SENSOR_COMBINED_SUB
+	int		_gyro_sub;			/**< gyro sensor subscription */
+	int		_accel_sub;			/**< accel sensor subscription */
+	int		_mag_sub;			/**< mag sensor subscription */
+#else
+	int		_sensor_combined_sub;
+#endif
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_baro_sub;			/**< barometer subscription */
+	int		_gps_sub;			/**< GPS subscription */
+	int		_vstatus_sub;			/**< vehicle status subscription */
+	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_control_sub;		/**< notification of manual control updates */
+	int		_mission_sub;
+	int		_home_sub;			/**< home position as defined by commander / user */
+
+	orb_advert_t	_att_pub;			/**< vehicle attitude */
+	orb_advert_t	_global_pos_pub;		/**< global position */
+	orb_advert_t	_local_pos_pub;			/**< position in local frame */
+	orb_advert_t	_estimator_status_pub;		/**< status of the estimator */
+	orb_advert_t	_wind_pub;			/**< wind estimate */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct gyro_report				_gyro;
+	struct accel_report				_accel;
+	struct mag_report				_mag;
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct baro_report				_baro;			/**< baro readings */
+	struct vehicle_status_s				_vstatus;		/**< vehicle status */
+	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
+	struct vehicle_local_position_s			_local_pos;		/**< local vehicle position */
+	struct vehicle_gps_position_s			_gps;			/**< GPS position */
+	struct wind_estimate_s				_wind;			/**< Wind estimate */
+
+	struct gyro_scale				_gyro_offsets;
+	struct accel_scale				_accel_offsets;
+	struct mag_scale				_mag_offsets;
+
+#ifdef SENSOR_COMBINED_SUB
+	struct sensor_combined_s			_sensor_combined;
+#endif
+
+	struct map_projection_reference_s	_pos_ref;
+
+	float						_baro_ref;		/**< barometer reference altitude */
+	float						_baro_ref_offset;	/**< offset between initial baro reference and GPS init baro altitude */
+	float						_baro_gps_offset;	/**< offset between baro altitude (at GPS init time) and GPS altitude */
+
+	perf_counter_t	_loop_perf;			///< loop performance counter
+	perf_counter_t	_perf_gyro;			///<local performance counter for gyro updates
+	perf_counter_t	_perf_mag;			///<local performance counter for mag updates
+	perf_counter_t	_perf_gps;			///<local performance counter for gps updates
+	perf_counter_t	_perf_baro;			///<local performance counter for baro updates
+	perf_counter_t	_perf_airspeed;			///<local performance counter for airspeed updates
+	perf_counter_t	_perf_reset;			///<local performance counter for filter resets
+
+	bool						_baro_init;
+	bool						_gps_initialized;
+	hrt_abstime					_gps_start_time;
+	hrt_abstime					_filter_start_time;
+	hrt_abstime					_last_sensor_timestamp;
+	hrt_abstime					_last_run;
+	bool						_gyro_valid;
+	bool						_accel_valid;
+	bool						_mag_valid;
+	bool						_ekf_logging;		///< log EKF state
+	unsigned					_debug;			///< debug level - default 0
+
+	int						_mavlink_fd;
+
+	struct {
+		int32_t	vel_delay_ms;
+		int32_t	pos_delay_ms;
+		int32_t	height_delay_ms;
+		int32_t	mag_delay_ms;
+		int32_t	tas_delay_ms;
+		float velne_noise;
+		float veld_noise;
+		float posne_noise;
+		float posd_noise;
+		float mag_noise;
+		float gyro_pnoise;
+		float acc_pnoise;
+		float gbias_pnoise;
+		float abias_pnoise;
+		float mage_pnoise;
+		float magb_pnoise;
+		float eas_noise;
+		float pos_stddev_threshold;
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	struct {
+		param_t vel_delay_ms;
+		param_t	pos_delay_ms;
+		param_t	height_delay_ms;
+		param_t	mag_delay_ms;
+		param_t	tas_delay_ms;
+		param_t velne_noise;
+		param_t veld_noise;
+		param_t posne_noise;
+		param_t posd_noise;
+		param_t mag_noise;
+		param_t gyro_pnoise;
+		param_t acc_pnoise;
+		param_t gbias_pnoise;
+		param_t abias_pnoise;
+		param_t mage_pnoise;
+		param_t magb_pnoise;
+		param_t eas_noise;
+		param_t pos_stddev_threshold;
+	}		_parameter_handles;		/**< handles for interesting parameters */
+
+	AttPosEKF					*_ekf;
+
+	float						_velocity_xy_filtered;
+	float						_velocity_z_filtered;
+	float						_airspeed_filtered;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int		parameters_update();
+
+	/**
+	 * Update control outputs
+	 *
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in vehicle status.
+	 */
+	void		vehicle_status_poll();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main filter task.
+	 */
+	void		task_main();
+
+	/**
+	 * Check filter sanity state
+	 *
+	 * @return zero if ok, non-zero for a filter error condition.
+	 */
+	int		check_filter_state();
+};
+
+namespace estimator
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+FixedwingEstimator	*g_estimator = nullptr;
+}
+
+FixedwingEstimator::FixedwingEstimator() :
+
+	_task_should_exit(false),
+	_task_running(false),
+	_estimator_task(-1),
+
+/* subscriptions */
+#ifndef SENSOR_COMBINED_SUB
+	_gyro_sub(-1),
+	_accel_sub(-1),
+	_mag_sub(-1),
+#else
+	_sensor_combined_sub(-1),
+#endif
+	_airspeed_sub(-1),
+	_baro_sub(-1),
+	_gps_sub(-1),
+	_vstatus_sub(-1),
+	_params_sub(-1),
+	_manual_control_sub(-1),
+	_mission_sub(-1),
+	_home_sub(-1),
+
+/* publications */
+	_att_pub(-1),
+	_global_pos_pub(-1),
+	_local_pos_pub(-1),
+	_estimator_status_pub(-1),
+	_wind_pub(-1),
+
+	_att({}),
+	_gyro({}),
+	_accel({}),
+	_mag({}),
+	_airspeed({}),
+	_baro({}),
+	_vstatus({}),
+	_global_pos({}),
+	_local_pos({}),
+	_gps({}),
+	_wind({}),
+
+	_gyro_offsets({}),
+	_accel_offsets({}),
+	_mag_offsets({}),
+
+	#ifdef SENSOR_COMBINED_SUB
+	_sensor_combined({}),
+	#endif
+
+	_baro_ref(0.0f),
+	_baro_ref_offset(0.0f),
+	_baro_gps_offset(0.0f),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+	_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
+	_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
+	_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
+	_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
+	_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
+	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
+
+/* states */
+	_baro_init(false),
+	_gps_initialized(false),
+	_gyro_valid(false),
+	_accel_valid(false),
+	_mag_valid(false),
+	_ekf_logging(true),
+	_debug(0),
+	_mavlink_fd(-1),
+	_ekf(nullptr),
+	_velocity_xy_filtered(0.0f),
+	_velocity_z_filtered(0.0f),
+	_airspeed_filtered(0.0f)
+{
+
+	_last_run = hrt_absolute_time();
+
+	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
+	_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
+	_parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
+	_parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS");
+	_parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS");
+	_parameter_handles.velne_noise = param_find("PE_VELNE_NOISE");
+	_parameter_handles.veld_noise = param_find("PE_VELD_NOISE");
+	_parameter_handles.posne_noise = param_find("PE_POSNE_NOISE");
+	_parameter_handles.posd_noise = param_find("PE_POSD_NOISE");
+	_parameter_handles.mag_noise = param_find("PE_MAG_NOISE");
+	_parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE");
+	_parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE");
+	_parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE");
+	_parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE");
+	_parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE");
+	_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
+	_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
+	_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
+
+	/* fetch initial parameter values */
+	parameters_update();
+
+	/* get offsets */
+
+	int fd, res;
+
+	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
+		close(fd);
+
+		if (res) {
+			warnx("G SCALE FAIL");
+		}
+	}
+
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
+		close(fd);
+
+		if (res) {
+			warnx("A SCALE FAIL");
+		}
+	}
+
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
+		close(fd);
+
+		if (res) {
+			warnx("M SCALE FAIL");
+		}
+	}
+}
+
+FixedwingEstimator::~FixedwingEstimator()
+{
+	if (_estimator_task != -1) {
+
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_estimator_task);
+				break;
+			}
+		} while (_estimator_task != -1);
+	}
+
+	delete _ekf;
+
+	estimator::g_estimator = nullptr;
+}
+
+int
+FixedwingEstimator::enable_logging(bool logging)
+{
+	_ekf_logging = logging;
+
+	return 0;
+}
+
+int
+FixedwingEstimator::parameters_update()
+{
+
+	param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms));
+	param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms));
+	param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms));
+	param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms));
+	param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms));
+	param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise));
+	param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise));
+	param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise));
+	param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise));
+	param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise));
+	param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise));
+	param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise));
+	param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise));
+	param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise));
+	param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise));
+	param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
+	param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
+	param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
+
+	if (_ekf) {
+		// _ekf->yawVarScale = 1.0f;
+		// _ekf->windVelSigma = 0.1f;
+		_ekf->dAngBiasSigma = _parameters.gbias_pnoise;
+		_ekf->dVelBiasSigma = _parameters.abias_pnoise;
+		_ekf->magEarthSigma = _parameters.mage_pnoise;
+		_ekf->magBodySigma  = _parameters.magb_pnoise;
+		// _ekf->gndHgtSigma  = 0.02f;
+		_ekf->vneSigma = _parameters.velne_noise;
+		_ekf->vdSigma = _parameters.veld_noise;
+		_ekf->posNeSigma = _parameters.posne_noise;
+		_ekf->posDSigma = _parameters.posd_noise;
+		_ekf->magMeasurementSigma = _parameters.mag_noise;
+		_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
+        	_ekf->accelProcessNoise = _parameters.acc_pnoise;
+		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
+	}
+
+	return OK;
+}
+
+void
+FixedwingEstimator::vehicle_status_poll()
+{
+	bool vstatus_updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_vstatus_sub, &vstatus_updated);
+
+	if (vstatus_updated) {
+
+		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
+	}
+}
+
+int
+FixedwingEstimator::check_filter_state()
+{
+	/*
+	 *    CHECK IF THE INPUT DATA IS SANE
+	 */
+
+	struct ekf_status_report ekf_report;
+
+	int check = _ekf->CheckAndBound(&ekf_report);
+
+	const char* const feedback[] = { 0,
+					"NaN in states, resetting",
+					"stale IMU data, resetting",
+					"got initial position lock",
+					"excessive gyro offsets",
+					"GPS velocity divergence",
+					"excessive covariances",
+					"unknown condition"};
+
+	// Print out error condition
+	if (check) {
+		unsigned warn_index = static_cast<unsigned>(check);
+		unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0]));
+
+		if (max_warn_index < warn_index) {
+			warn_index = max_warn_index;
+		}
+
+		warnx("reset: %s", feedback[warn_index]);
+		mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]);
+	}
+
+	struct estimator_status_report rep;
+	memset(&rep, 0, sizeof(rep));
+
+	// If error flag is set, we got a filter reset
+	if (check && ekf_report.error) {
+
+		// Count the reset condition
+		perf_count(_perf_reset);
+
+	} else if (_ekf_logging) {
+		_ekf->GetFilterState(&ekf_report);
+	}
+
+	if (_ekf_logging || check) {
+		rep.timestamp = hrt_absolute_time();
+
+		rep.nan_flags |= (((uint8_t)ekf_report.angNaN)		<< 0);
+		rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN)	<< 1);
+		rep.nan_flags |= (((uint8_t)ekf_report.KHNaN)		<< 2);
+		rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN)		<< 3);
+		rep.nan_flags |= (((uint8_t)ekf_report.PNaN)		<< 4);
+		rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN)	<< 5);
+		rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN)	<< 6);
+		rep.nan_flags |= (((uint8_t)ekf_report.statesNaN)	<< 7);
+
+		rep.health_flags |= (((uint8_t)ekf_report.velHealth)	<< 0);
+		rep.health_flags |= (((uint8_t)ekf_report.posHealth)	<< 1);
+		rep.health_flags |= (((uint8_t)ekf_report.hgtHealth)	<< 2);
+		rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive)	<< 3);
+		// rep.health_flags |= (((uint8_t)ekf_report.onGround)	<< 4);
+		// rep.health_flags |= (((uint8_t)ekf_report.staticMode)	<< 5);
+		// rep.health_flags |= (((uint8_t)ekf_report.useCompass)	<< 6);
+		// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed)	<< 7);
+
+		rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout)	<< 0);
+		rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout)	<< 1);
+		rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout)	<< 2);
+		rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout)	<< 3);
+
+		if (_debug > 10) {
+
+			if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) {
+				warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s",
+					((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
+					((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
+					((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
+					((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
+			}
+
+			if (rep.timeout_flags) {
+				warnx("timeout: %s%s%s%s",
+					((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
+					((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
+					((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
+					((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
+			}
+		}
+
+		// Copy all states or at least all that we can fit
+		unsigned ekf_n_states = ekf_report.n_states;
+		unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
+		rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
+
+		for (unsigned i = 0; i < rep.n_states; i++) {
+			rep.states[i] = ekf_report.states[i];
+		}
+
+		for (unsigned i = 0; i < rep.n_states; i++) {
+			rep.states[i] = ekf_report.states[i];
+		}
+
+		if (_estimator_status_pub > 0) {
+			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
+		} else {
+			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
+		}
+	}
+
+	return check;
+}
+
+void
+FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
+{
+	estimator::g_estimator->task_main();
+}
+
+void
+FixedwingEstimator::task_main()
+{
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	_ekf = new AttPosEKF();
+	float dt = 0.0f; // time lapsed since last covariance prediction
+	_filter_start_time = hrt_absolute_time();
+
+	if (!_ekf) {
+		errx(1, "OUT OF MEM!");
+	}
+
+	/*
+	 * do subscriptions
+	 */
+	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_home_sub = orb_subscribe(ORB_ID(home_position));
+
+	/* rate limit vehicle status updates to 5Hz */
+	orb_set_interval(_vstatus_sub, 200);
+
+#ifndef SENSOR_COMBINED_SUB
+
+	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+
+	/* rate limit gyro updates to 50 Hz */
+	/* XXX remove this!, BUT increase the data buffer size! */
+	orb_set_interval(_gyro_sub, 4);
+#else
+	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	/* XXX remove this!, BUT increase the data buffer size! */
+	orb_set_interval(_sensor_combined_sub, 9);
+#endif
+
+	/* sets also parameters in the EKF object */
+	parameters_update();
+
+	Vector3f lastAngRate;
+	Vector3f lastAccel;
+
+	/* wakeup source(s) */
+	struct pollfd fds[2];
+
+	/* Setup of loop */
+	fds[0].fd = _params_sub;
+	fds[0].events = POLLIN;
+#ifndef SENSOR_COMBINED_SUB
+	fds[1].fd = _gyro_sub;
+	fds[1].events = POLLIN;
+#else
+	fds[1].fd = _sensor_combined_sub;
+	fds[1].events = POLLIN;
+#endif
+
+	bool newDataGps = false;
+	bool newHgtData = false;
+	bool newAdsData = false;
+	bool newDataMag = false;
+
+	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
+
+	uint64_t last_gps = 0;
+	_gps.vel_n_m_s = 0.0f;
+	_gps.vel_e_m_s = 0.0f;
+	_gps.vel_d_m_s = 0.0f;
+
+	_task_running = true;
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 500ms 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)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("POLL ERR %d, %d", pret, errno);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* only update parameters if they changed */
+		if (fds[0].revents & POLLIN) {
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		/* only run estimator if gyro updated */
+		if (fds[1].revents & POLLIN) {
+
+			/* check vehicle status for changes to publication state */
+			bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
+			vehicle_status_poll();
+
+			bool accel_updated;
+			bool mag_updated;
+
+			perf_count(_perf_gyro);
+
+			/* Reset baro reference if switching to HIL, reset sensor states */
+			if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
+				/* system is in HIL now, wait for measurements to come in one last round */
+				usleep(60000);
+
+#ifndef SENSOR_COMBINED_SUB
+				orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
+				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
+#else
+				/* now read all sensor publications to ensure all real sensor data is purged */
+				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+#endif
+
+				/* set sensors to de-initialized state */
+				_gyro_valid = false;
+				_accel_valid = false;
+				_mag_valid = false;
+
+				_baro_init = false;
+				_gps_initialized = false;
+				_last_sensor_timestamp = hrt_absolute_time();
+				_last_run = _last_sensor_timestamp;
+
+				_ekf->ZeroVariables();
+				_ekf->dtIMU = 0.01f;
+				_filter_start_time = _last_sensor_timestamp;
+
+				/* now skip this loop and get data on the next one, which will also re-init the filter */
+				continue;
+			}
+
+			/**
+			 *    PART ONE: COLLECT ALL DATA
+			 **/
+
+			/* load local copies */
+#ifndef SENSOR_COMBINED_SUB
+			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
+
+
+			orb_check(_accel_sub, &accel_updated);
+
+			if (accel_updated) {
+				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+			}
+
+			_last_sensor_timestamp = _gyro.timestamp;
+			IMUmsec = _gyro.timestamp / 1e3f;
+
+			float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
+			_last_run = _gyro.timestamp;
+
+			/* guard against too large deltaT's */
+			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+				deltaT = 0.01f;
+			}
+
+
+			// Always store data, independent of init status
+			/* fill in last data set */
+			_ekf->dtIMU = deltaT;
+
+			if (isfinite(_gyro.x) &&
+				isfinite(_gyro.y) &&
+				isfinite(_gyro.z)) {
+				_ekf->angRate.x = _gyro.x;
+				_ekf->angRate.y = _gyro.y;
+				_ekf->angRate.z = _gyro.z;
+
+				if (!_gyro_valid) {
+					lastAngRate = _ekf->angRate;
+				}
+
+				_gyro_valid = true;
+			}
+
+			if (accel_updated) {
+				_ekf->accel.x = _accel.x;
+				_ekf->accel.y = _accel.y;
+				_ekf->accel.z = _accel.z;
+
+				if (!_accel_valid) {
+					lastAccel = _ekf->accel;
+				}
+
+				_accel_valid = true;
+			}
+
+			_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
+			_ekf->lastAngRate = angRate;
+			_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
+			_ekf->lastAccel = accel;
+
+
+#else
+			orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+
+			static hrt_abstime last_accel = 0;
+			static hrt_abstime last_mag = 0;
+
+			if (last_accel != _sensor_combined.accelerometer_timestamp) {
+				accel_updated = true;
+			} else {
+				accel_updated = false;
+			}
+
+			last_accel = _sensor_combined.accelerometer_timestamp;
+
+
+			// Copy gyro and accel
+			_last_sensor_timestamp = _sensor_combined.timestamp;
+			IMUmsec = _sensor_combined.timestamp / 1e3f;
+
+			float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
+
+			/* guard against too large deltaT's */
+			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+				deltaT = 0.01f;
+			}
+
+			_last_run = _sensor_combined.timestamp;
+
+			// Always store data, independent of init status
+			/* fill in last data set */
+			_ekf->dtIMU = deltaT;
+
+			if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
+				isfinite(_sensor_combined.gyro_rad_s[1]) &&
+				isfinite(_sensor_combined.gyro_rad_s[2])) {
+				_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
+				_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
+				_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
+
+				if (!_gyro_valid) {
+					lastAngRate = _ekf->angRate;
+				}
+
+				_gyro_valid = true;
+				perf_count(_perf_gyro);
+			}
+
+			if (accel_updated) {
+				_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
+				_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
+				_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
+
+				if (!_accel_valid) {
+					lastAccel = _ekf->accel;
+				}
+
+				_accel_valid = true;
+			}
+
+			_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
+			lastAngRate = _ekf->angRate;
+			_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
+			lastAccel = _ekf->accel;
+
+			if (last_mag != _sensor_combined.magnetometer_timestamp) {
+				mag_updated = true;
+				newDataMag = true;
+
+			} else {
+				newDataMag = false;
+			}
+
+			last_mag = _sensor_combined.magnetometer_timestamp;
+
+#endif
+
+			//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
+
+			bool airspeed_updated;
+			orb_check(_airspeed_sub, &airspeed_updated);
+
+			if (airspeed_updated) {
+				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+				perf_count(_perf_airspeed);
+
+				_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
+				newAdsData = true;
+
+			} else {
+				newAdsData = false;
+			}
+
+			bool gps_updated;
+			orb_check(_gps_sub, &gps_updated);
+
+			if (gps_updated) {
+
+				last_gps = _gps.timestamp_position;
+
+				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
+				perf_count(_perf_gps);
+
+				if (_gps.fix_type < 3) {
+					newDataGps = false;
+
+				} else {
+
+					/* store time of valid GPS measurement */
+					_gps_start_time = hrt_absolute_time();
+
+					/* check if we had a GPS outage for a long time */
+					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
+						_ekf->ResetPosition();
+						_ekf->ResetVelocity();
+						_ekf->ResetStoredStates();
+					}
+
+					/* fuse GPS updates */
+
+					//_gps.timestamp / 1e3;
+					_ekf->GPSstatus = _gps.fix_type;
+					_ekf->velNED[0] = _gps.vel_n_m_s;
+					_ekf->velNED[1] = _gps.vel_e_m_s;
+					_ekf->velNED[2] = _gps.vel_d_m_s;
+
+					// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
+
+					_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
+					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
+					_ekf->gpsHgt = _gps.alt / 1e3f;
+
+					// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
+					// 	_ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
+					// } else {
+					// 	_ekf->vneSigma = _parameters.velne_noise;
+					// }
+
+					// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
+					// 	_ekf->posNeSigma = sqrtf(_gps.p_variance_m);
+					// } else {
+					// 	_ekf->posNeSigma = _parameters.posne_noise;
+					// }
+
+					// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
+
+					newDataGps = true;
+
+				}
+
+			}
+
+			bool baro_updated;
+			orb_check(_baro_sub, &baro_updated);
+
+			if (baro_updated) {
+				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+
+				_ekf->baroHgt = _baro.altitude;
+
+				if (!_baro_init) {
+					_baro_ref = _baro.altitude;
+					_baro_init = true;
+					warnx("ALT REF INIT");
+				}
+
+				perf_count(_perf_baro);
+
+				newHgtData = true;
+			} else {
+				newHgtData = false;
+			}
+
+#ifndef SENSOR_COMBINED_SUB
+			orb_check(_mag_sub, &mag_updated);
+#endif
+
+			if (mag_updated) {
+
+				_mag_valid = true;
+
+				perf_count(_perf_mag);
+
+#ifndef SENSOR_COMBINED_SUB
+				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
+
+				// XXX we compensate the offsets upfront - should be close to zero.
+				// 0.001f
+				_ekf->magData.x = _mag.x;
+				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+				_ekf->magData.y = _mag.y;
+				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+				_ekf->magData.z = _mag.z;
+				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+
+#else
+
+				// XXX we compensate the offsets upfront - should be close to zero.
+				// 0.001f
+				_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
+				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+				_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
+				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+				_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
+				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+
+#endif
+
+				newDataMag = true;
+
+			} else {
+				newDataMag = false;
+			}
+
+			/*
+			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
+			 */
+			if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
+				continue;
+			}
+
+			/**
+			 *    PART TWO: EXECUTE THE FILTER
+			 *
+			 *    We run the filter only once all data has been fetched
+			 **/
+
+			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
+
+				float initVelNED[3];
+
+				/* Initialize the filter first */
+				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
+
+					// GPS is in scaled integers, convert
+					double lat = _gps.lat / 1.0e7;
+					double lon = _gps.lon / 1.0e7;
+					float gps_alt = _gps.alt / 1e3f;
+
+					initVelNED[0] = _gps.vel_n_m_s;
+					initVelNED[1] = _gps.vel_e_m_s;
+					initVelNED[2] = _gps.vel_d_m_s;
+
+					// Set up height correctly
+					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
+					_baro_gps_offset = _baro.altitude - gps_alt;
+					_ekf->baroHgt = _baro.altitude;
+					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
+
+					// Set up position variables correctly
+					_ekf->GPSstatus = _gps.fix_type;
+
+					_ekf->gpsLat = math::radians(lat);
+					_ekf->gpsLon = math::radians(lon) - M_PI;
+					_ekf->gpsHgt = gps_alt;
+
+					// Look up mag declination based on current position
+					float declination = math::radians(get_mag_declination(lat, lon));
+
+					_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
+
+					// Initialize projection
+					_local_pos.ref_lat = lat;
+					_local_pos.ref_lon = lon;
+					_local_pos.ref_alt = gps_alt;
+					_local_pos.ref_timestamp = _gps.timestamp_position;
+
+					map_projection_init(&_pos_ref, lat, lon);
+					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
+
+					#if 0
+					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("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset);
+					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
+					#endif
+
+					_gps_initialized = true;
+
+				} else if (!_ekf->statesInitialised) {
+
+					initVelNED[0] = 0.0f;
+					initVelNED[1] = 0.0f;
+					initVelNED[2] = 0.0f;
+					_ekf->posNE[0] = posNED[0];
+					_ekf->posNE[1] = posNED[1];
+
+					_local_pos.ref_alt = _baro_ref;
+					_baro_ref_offset = 0.0f;
+					_baro_gps_offset = 0.0f;
+
+					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
+
+				} else if (_ekf->statesInitialised) {
+
+					// We're apparently initialized in this case now
+					int check = check_filter_state();
+
+					if (check) {
+						// Let the system re-initialize itself
+						continue;
+					}
+
+					// Run the strapdown INS equations every IMU update
+					_ekf->UpdateStrapdownEquationsNED();
+	#if 0
+					// debug code - could be tunred into a filter mnitoring/watchdog function
+					float tempQuat[4];
+
+					for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
+
+					quat2eul(eulerEst, tempQuat);
+
+					for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
+
+					if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
+
+					if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
+
+	#endif
+					// store the predicted states for subsequent use by measurement fusion
+					_ekf->StoreStates(IMUmsec);
+					// Check if on ground - status is used by covariance prediction
+					_ekf->OnGroundCheck();
+					// sum delta angles and time used by covariance prediction
+					_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
+					_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
+					dt += _ekf->dtIMU;
+
+					// perform a covariance prediction if the total delta angle has exceeded the limit
+					// or the time limit will be exceeded at the next IMU update
+					if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
+						_ekf->CovariancePrediction(dt);
+						_ekf->summedDelAng.zero();
+						_ekf->summedDelVel.zero();
+						dt = 0.0f;
+					}
+
+					// Fuse GPS Measurements
+					if (newDataGps && _gps_initialized) {
+						// Convert GPS measurements to Pos NE, hgt and Vel NED
+
+						float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f;
+
+						// Calculate acceleration predicted by GPS velocity change
+						if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) ||
+							(fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) ||
+							(fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) {
+
+							_ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt;
+							_ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt;
+							_ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt;
+						}
+
+						_ekf->velNED[0] = _gps.vel_n_m_s;
+						_ekf->velNED[1] = _gps.vel_e_m_s;
+						_ekf->velNED[2] = _gps.vel_d_m_s;
+						_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
+
+						_ekf->posNE[0] = posNED[0];
+						_ekf->posNE[1] = posNED[1];
+						// set fusion flags
+						_ekf->fuseVelData = true;
+						_ekf->fusePosData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
+
+					} else if (!_gps_initialized) {
+
+						// force static mode
+						_ekf->staticMode = true;
+
+						// Convert GPS measurements to Pos NE, hgt and Vel NED
+						_ekf->velNED[0] = 0.0f;
+						_ekf->velNED[1] = 0.0f;
+						_ekf->velNED[2] = 0.0f;
+
+						_ekf->posNE[0] = 0.0f;
+						_ekf->posNE[1] = 0.0f;
+						// set fusion flags
+						_ekf->fuseVelData = true;
+						_ekf->fusePosData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
+
+					} else {
+						_ekf->fuseVelData = false;
+						_ekf->fusePosData = false;
+					}
+
+					if (newHgtData) {
+						// Could use a blend of GPS and baro alt data if desired
+						_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
+						_ekf->fuseHgtData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
+
+					} else {
+						_ekf->fuseHgtData = false;
+					}
+
+					// Fuse Magnetometer Measurements
+					if (newDataMag) {
+						_ekf->fuseMagData = true;
+						_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+
+						_ekf->magstate.obsIndex = 0;
+						_ekf->FuseMagnetometer();
+						_ekf->FuseMagnetometer();
+						_ekf->FuseMagnetometer();
+
+					} else {
+						_ekf->fuseMagData = false;
+					}
+
+					// Fuse Airspeed Measurements
+					if (newAdsData && _ekf->VtasMeas > 7.0f) {
+						_ekf->fuseVtasData = true;
+						_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+						_ekf->FuseAirspeed();
+
+					} else {
+						_ekf->fuseVtasData = false;
+					}
+
+
+					// Output results
+					math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+					math::Matrix<3, 3> R = q.to_dcm();
+					math::Vector<3> euler = R.to_euler();
+
+					for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+							_att.R[i][j] = R(i, j);
+
+					_att.timestamp = _last_sensor_timestamp;
+					_att.q[0] = _ekf->states[0];
+					_att.q[1] = _ekf->states[1];
+					_att.q[2] = _ekf->states[2];
+					_att.q[3] = _ekf->states[3];
+					_att.q_valid = true;
+					_att.R_valid = true;
+
+					_att.timestamp = _last_sensor_timestamp;
+					_att.roll = euler(0);
+					_att.pitch = euler(1);
+					_att.yaw = euler(2);
+
+					_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
+					_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
+					_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+					// gyro offsets
+					_att.rate_offsets[0] = _ekf->states[10];
+					_att.rate_offsets[1] = _ekf->states[11];
+					_att.rate_offsets[2] = _ekf->states[12];
+
+					/* lazily publish the attitude only once available */
+					if (_att_pub > 0) {
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
+
+					} else {
+						/* advertise and publish */
+						_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
+					}
+
+					if (_gps_initialized) {
+						_local_pos.timestamp = _last_sensor_timestamp;
+						_local_pos.x = _ekf->states[7];
+						_local_pos.y = _ekf->states[8];
+						// XXX need to announce change of Z reference somehow elegantly
+						_local_pos.z = _ekf->states[9] - _baro_ref_offset;
+
+						_local_pos.vx = _ekf->states[4];
+						_local_pos.vy = _ekf->states[5];
+						_local_pos.vz = _ekf->states[6];
+
+						_local_pos.xy_valid = _gps_initialized;
+						_local_pos.z_valid = true;
+						_local_pos.v_xy_valid = _gps_initialized;
+						_local_pos.v_z_valid = true;
+						_local_pos.xy_global = true;
+
+						_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.95f*_airspeed_filtered + 0.05f*_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 < 5
+							&& _velocity_z_filtered < 10
+							&& _airspeed_filtered < 10) {
+							_local_pos.landed = true;
+						} else {
+							_local_pos.landed = false;
+						}
+
+						_local_pos.z_global = false;
+						_local_pos.yaw = _att.yaw;
+
+						/* lazily publish the local position only once available */
+						if (_local_pos_pub > 0) {
+							/* publish the attitude setpoint */
+							orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
+
+						} else {
+							/* advertise and publish */
+							_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
+						}
+
+						_global_pos.timestamp = _local_pos.timestamp;
+
+						if (_local_pos.xy_global) {
+							double est_lat, est_lon;
+							map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
+							_global_pos.lat = est_lat;
+							_global_pos.lon = est_lon;
+							_global_pos.time_gps_usec = _gps.time_gps_usec;
+							_global_pos.eph = _gps.eph;
+							_global_pos.epv = _gps.epv;
+						}
+
+						if (_local_pos.v_xy_valid) {
+							_global_pos.vel_n = _local_pos.vx;
+							_global_pos.vel_e = _local_pos.vy;
+						} else {
+							_global_pos.vel_n = 0.0f;
+							_global_pos.vel_e = 0.0f;
+						}
+
+						/* local pos alt is negative, change sign and add alt offsets */
+						_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
+
+						if (_local_pos.v_z_valid) {
+							_global_pos.vel_d = _local_pos.vz;
+						}
+
+
+						_global_pos.yaw = _local_pos.yaw;
+
+						_global_pos.eph = _gps.eph;
+						_global_pos.epv = _gps.epv;
+
+						_global_pos.timestamp = _local_pos.timestamp;
+
+						/* lazily publish the global position only once available */
+						if (_global_pos_pub > 0) {
+							/* publish the global position */
+							orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
+
+						} else {
+							/* advertise and publish */
+							_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+						}
+
+						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+							_wind.timestamp = _global_pos.timestamp;
+							_wind.windspeed_north = _ekf->states[14];
+							_wind.windspeed_east = _ekf->states[15];
+							_wind.covariance_north = 0.0f; // XXX get form filter
+							_wind.covariance_east = 0.0f;
+
+							/* lazily publish the wind estimate only once available */
+							if (_wind_pub > 0) {
+								/* publish the wind estimate */
+								orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+
+							} else {
+								/* advertise and publish */
+								_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+							}
+
+						}
+
+						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+							_wind.timestamp = _global_pos.timestamp;
+							_wind.windspeed_north = _ekf->states[14];
+							_wind.windspeed_east = _ekf->states[15];
+							_wind.covariance_north = _ekf->P[14][14];
+							_wind.covariance_east = _ekf->P[15][15];
+
+							/* lazily publish the wind estimate only once available */
+							if (_wind_pub > 0) {
+								/* publish the wind estimate */
+								orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+
+							} else {
+								/* advertise and publish */
+								_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+							}
+						}
+
+					}
+
+				}
+
+			}
+
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	_task_running = false;
+
+	warnx("exiting.\n");
+
+	_estimator_task = -1;
+	_exit(0);
+}
+
+int
+FixedwingEstimator::start()
+{
+	ASSERT(_estimator_task == -1);
+
+	/* start the task */
+	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 40,
+					 5000,
+					 (main_t)&FixedwingEstimator::task_main_trampoline,
+					 nullptr);
+
+	if (_estimator_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+FixedwingEstimator::print_status()
+{
+	math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+	math::Matrix<3, 3> R = q.to_dcm();
+	math::Vector<3> euler = R.to_euler();
+
+	printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
+	       (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
+
+	// State vector:
+	// 0-3: quaternions (q0, q1, q2, q3)
+	// 4-6: Velocity - m/sec (North, East, Down)
+	// 7-9: Position - m (North, East, Down)
+	// 10-12: Delta Angle bias - rad (X,Y,Z)
+	// 13:    Accelerometer offset
+	// 14-15: Wind Vector  - m/sec (North,East)
+	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
+	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
+	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
+	printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset);
+	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
+	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
+	printf("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
+	printf("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
+	printf("states (pos m)      [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
+	printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
+
+	if (n_states == 23) {
+		printf("states (accel offs)   [13]: %8.4f\n", (double)_ekf->states[13]);
+		printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
+		printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]);
+		printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]);
+		printf("states (terrain)      [22]: %8.4f\n", (double)_ekf->states[22]);
+
+	} else {
+		printf("states (wind)      [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
+		printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
+		printf("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+	}
+	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
+		       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
+		       (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
+		       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
+		       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
+		       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
+		       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
+		       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
+		       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
+		       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
+		       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
+}
+
+int FixedwingEstimator::trip_nan() {
+
+	int ret = 0;
+
+	// If system is not armed, inject a NaN value into the filter
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	struct actuator_armed_s armed;
+	orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+
+	if (armed.armed) {
+		warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
+		ret = 1;
+	} else {
+
+		float nan_val = 0.0f / 0.0f;
+
+		warnx("system not armed, tripping state vector with NaN values");
+		_ekf->states[5] = nan_val;
+		usleep(100000);
+
+		warnx("tripping covariance #1 with NaN values");
+		_ekf->KH[2][2] = nan_val; //  intermediate result used for covariance updates
+		usleep(100000);
+
+		warnx("tripping covariance #2 with NaN values");
+		_ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
+		usleep(100000);
+
+		warnx("tripping covariance #3 with NaN values");
+		_ekf->P[3][3] = nan_val; // covariance matrix
+		usleep(100000);
+
+		warnx("tripping Kalman gains with NaN values");
+		_ekf->Kfusion[0] = nan_val; // Kalman gains
+		usleep(100000);
+
+		warnx("tripping stored states[0] with NaN values");
+		_ekf->storedStates[0][0] = nan_val;
+		usleep(100000);
+
+		warnx("\nDONE - FILTER STATE:");
+		print_status();
+	}
+
+	close(armed_sub);
+	return ret;
+}
+
+int ekf_att_pos_estimator_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (estimator::g_estimator != nullptr)
+			errx(1, "already running");
+
+		estimator::g_estimator = new FixedwingEstimator;
+
+		if (estimator::g_estimator == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != estimator::g_estimator->start()) {
+			delete estimator::g_estimator;
+			estimator::g_estimator = nullptr;
+			err(1, "start failed");
+		}
+
+		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
+		while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) {
+			usleep(50000);
+			printf(".");
+			fflush(stdout);
+		}
+		printf("\n");
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (estimator::g_estimator == nullptr)
+			errx(1, "not running");
+
+		delete estimator::g_estimator;
+		estimator::g_estimator = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (estimator::g_estimator) {
+			warnx("running");
+
+			estimator::g_estimator->print_status();
+
+			exit(0);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	if (!strcmp(argv[1], "trip")) {
+		if (estimator::g_estimator) {
+			int ret = estimator::g_estimator->trip_nan();
+
+			exit(ret);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	if (!strcmp(argv[1], "logging")) {
+		if (estimator::g_estimator) {
+			int ret = estimator::g_estimator->enable_logging(true);
+
+			exit(ret);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	if (!strcmp(argv[1], "debug")) {
+		if (estimator::g_estimator) {
+			int debug = strtoul(argv[2], NULL, 10);
+			int ret = estimator::g_estimator->set_debuglevel(debug);
+
+			exit(ret);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
new file mode 100644
index 000000000..8c82dd6c1
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
@@ -0,0 +1,271 @@
+/****************************************************************************
+ *
+ *   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 ekf_att_pos_estimator_params.c
+ *
+ * Parameters defined by the attitude and position estimator task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Estimator parameters, accessible via MAVLink
+ *
+ */
+
+/**
+ * Velocity estimate delay
+ *
+ * The delay in milliseconds of the velocity estimate from GPS.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
+
+/**
+ * Position estimate delay
+ *
+ * The delay in milliseconds of the position estimate from GPS.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
+
+/**
+ * Height estimate delay
+ *
+ * The delay in milliseconds of the height estimate from the barometer.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
+
+/**
+ * Mag estimate delay
+ *
+ * The delay in milliseconds of the magnetic field estimate from
+ * the magnetometer.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
+
+/**
+ * True airspeeed estimate delay
+ *
+ * The delay in milliseconds of the airspeed estimate.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
+
+/**
+ * GPS vs. barometric altitude update weight
+ *
+ * RE-CHECK this.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
+
+/**
+ * Airspeed measurement noise.
+ *
+ * Increasing this value will make the filter trust this sensor
+ * less and trust other sensors more.
+ *
+ * @min 0.5
+ * @max 5.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
+
+/**
+ * Velocity measurement noise in north-east (horizontal) direction.
+ *
+ * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
+ *
+ * @min 0.05
+ * @max 5.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
+
+/**
+ * Velocity noise in down (vertical) direction
+ *
+ * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7
+ *
+ * @min 0.05
+ * @max 5.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f);
+
+/**
+ * Position noise in north-east (horizontal) direction
+ *
+ * Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
+ *
+ * @min 0.1
+ * @max 10.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
+
+/**
+ * Position noise in down (vertical) direction
+ *
+ * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0
+ *
+ * @min 0.1
+ * @max 10.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f);
+
+/**
+ * Magnetometer measurement noise
+ *
+ * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
+ *
+ * @min 0.1
+ * @max 10.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
+
+/**
+ * Gyro process noise
+ *
+ * Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
+ * This noise controls how much the filter trusts the gyro measurements.
+ * Increasing it makes the filter trust the gyro less and other sensors more.
+ *
+ * @min 0.001
+ * @max 0.05
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
+
+/**
+ * Accelerometer process noise
+ *
+ * Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
+ * Increasing this value makes the filter trust the accelerometer less
+ * and other sensors more.
+ *
+ * @min 0.05
+ * @max 1.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f);
+
+/**
+ * Gyro bias estimate process noise
+ *
+ * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
+ * Increasing this value will make the gyro bias converge faster but noisier.
+ *
+ * @min 0.0000001
+ * @max 0.00001
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
+
+/**
+ * Accelerometer bias estimate process noise
+ *
+ * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
+ * Increasing this value makes the bias estimation faster and noisier.
+ *
+ * @min 0.00001
+ * @max 0.001
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f);
+
+/**
+ * Magnetometer earth frame offsets process noise
+ *
+ * Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
+ * Increasing this value makes the magnetometer earth bias estimate converge
+ * faster but also noisier.
+ *
+ * @min 0.0001
+ * @max 0.01
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
+
+/**
+ * Magnetometer body frame offsets process noise
+ *
+ * Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
+ * Increasing this value makes the magnetometer body bias estimate converge faster
+ * but also noisier.
+ *
+ * @min 0.0001
+ * @max 0.01
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
+
+/**
+ * Threshold for filter initialization.
+ *
+ * If the standard deviation of the GPS position estimate is below this threshold
+ * in meters, the filter will initialize.
+ *
+ * @min 0.3
+ * @max 10.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
new file mode 100644
index 000000000..67bfec4ea
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
@@ -0,0 +1,2142 @@
+#include "estimator_21states.h"
+
+#include <string.h>
+
+AttPosEKF::AttPosEKF() :
+    fusionModeGPS(0),
+    covSkipCount(0),
+    EAS2TAS(1.0f),
+    statesInitialised(false),
+    fuseVelData(false),
+    fusePosData(false),
+    fuseHgtData(false),
+    fuseMagData(false),
+    fuseVtasData(false),
+    onGround(true),
+    staticMode(true),
+    useAirspeed(true),
+    useCompass(true),
+    numericalProtection(true),
+    storeIndex(0),
+    magDeclination(0.0f)
+{
+    InitialiseParameters();
+}
+
+AttPosEKF::~AttPosEKF()
+{
+}
+
+void AttPosEKF::UpdateStrapdownEquationsNED()
+{
+    Vector3f delVelNav;
+    float q00;
+    float q11;
+    float q22;
+    float q33;
+    float q01;
+    float q02;
+    float q03;
+    float q12;
+    float q13;
+    float q23;
+    Mat3f Tbn;
+    Mat3f Tnb;
+    float rotationMag;
+    float qUpdated[4];
+    float quatMag;
+    double deltaQuat[4];
+    const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS};
+
+// Remove sensor bias errors
+    correctedDelAng.x = dAngIMU.x - states[10];
+    correctedDelAng.y = dAngIMU.y - states[11];
+    correctedDelAng.z = dAngIMU.z - states[12];
+    dVelIMU.x = dVelIMU.x;
+    dVelIMU.y = dVelIMU.y;
+    dVelIMU.z = dVelIMU.z;
+
+// Save current measurements
+    Vector3f  prevDelAng = correctedDelAng;
+
+// Apply corrections for earths rotation rate and coning errors
+// * and + operators have been overloaded
+    correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
+// Convert the rotation vector to its equivalent quaternion
+    rotationMag = correctedDelAng.length();
+    if (rotationMag < 1e-12f)
+    {
+        deltaQuat[0] = 1.0;
+        deltaQuat[1] = 0.0;
+        deltaQuat[2] = 0.0;
+        deltaQuat[3] = 0.0;
+    }
+    else
+    {
+        deltaQuat[0] = cos(0.5f*rotationMag);
+        double rotScaler = (sin(0.5f*rotationMag))/rotationMag;
+        deltaQuat[1] = correctedDelAng.x*rotScaler;
+        deltaQuat[2] = correctedDelAng.y*rotScaler;
+        deltaQuat[3] = correctedDelAng.z*rotScaler;
+    }
+
+// Update the quaternions by rotating from the previous attitude through
+// the delta angle rotation quaternion
+    qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
+    qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
+    qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
+    qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
+
+// Normalise the quaternions and update the quaternion states
+    quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
+    if (quatMag > 1e-16f)
+    {
+        float quatMagInv = 1.0f/quatMag;
+        states[0] = quatMagInv*qUpdated[0];
+        states[1] = quatMagInv*qUpdated[1];
+        states[2] = quatMagInv*qUpdated[2];
+        states[3] = quatMagInv*qUpdated[3];
+    }
+
+// Calculate the body to nav cosine matrix
+    q00 = sq(states[0]);
+    q11 = sq(states[1]);
+    q22 = sq(states[2]);
+    q33 = sq(states[3]);
+    q01 =  states[0]*states[1];
+    q02 =  states[0]*states[2];
+    q03 =  states[0]*states[3];
+    q12 =  states[1]*states[2];
+    q13 =  states[1]*states[3];
+    q23 =  states[2]*states[3];
+
+    Tbn.x.x = q00 + q11 - q22 - q33;
+    Tbn.y.y = q00 - q11 + q22 - q33;
+    Tbn.z.z = q00 - q11 - q22 + q33;
+    Tbn.x.y = 2*(q12 - q03);
+    Tbn.x.z = 2*(q13 + q02);
+    Tbn.y.x = 2*(q12 + q03);
+    Tbn.y.z = 2*(q23 - q01);
+    Tbn.z.x = 2*(q13 - q02);
+    Tbn.z.y = 2*(q23 + q01);
+
+    Tnb = Tbn.transpose();
+
+// transform body delta velocities to delta velocities in the nav frame
+// * and + operators have been overloaded
+    //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
+    delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
+    delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
+    delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
+
+// calculate the magnitude of the nav acceleration (required for GPS
+// variance estimation)
+    accNavMag = delVelNav.length()/dtIMU;
+
+// If calculating position save previous velocity
+    float lastVelocity[3];
+    lastVelocity[0] = states[4];
+    lastVelocity[1] = states[5];
+    lastVelocity[2] = states[6];
+
+// Sum delta velocities to get velocity
+    states[4] = states[4] + delVelNav.x;
+    states[5] = states[5] + delVelNav.y;
+    states[6] = states[6] + delVelNav.z;
+
+// If calculating postions, do a trapezoidal integration for position
+    states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
+    states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
+    states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
+
+    // Constrain states (to protect against filter divergence)
+    //ConstrainStates();
+}
+
+void AttPosEKF::CovariancePrediction(float dt)
+{
+    // scalars
+    float daxCov;
+    float dayCov;
+    float dazCov;
+    float dvxCov;
+    float dvyCov;
+    float dvzCov;
+    float dvx;
+    float dvy;
+    float dvz;
+    float dax;
+    float day;
+    float daz;
+    float q0;
+    float q1;
+    float q2;
+    float q3;
+    float dax_b;
+    float day_b;
+    float daz_b;
+
+    // arrays
+    float processNoise[21];
+    float SF[14];
+    float SG[8];
+    float SQ[11];
+    float SPP[13] = {0};
+    float nextP[21][21];
+
+    // calculate covariance prediction process noise
+    windVelSigma = dt*0.1f;
+    dAngBiasSigma = dt*5.0e-7f;
+    magEarthSigma = dt*3.0e-4f;
+    magBodySigma  = dt*3.0e-4f;
+    for (uint8_t i= 0; i<=9; i++) processNoise[i]  = 1.0e-9f;
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
+    if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale;
+    for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma;
+    for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma;
+    for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma;
+    for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]);
+
+    // set variables used to calculate covariance growth
+    dvx = summedDelVel.x;
+    dvy = summedDelVel.y;
+    dvz = summedDelVel.z;
+    dax = summedDelAng.x;
+    day = summedDelAng.y;
+    daz = summedDelAng.z;
+    q0 = states[0];
+    q1 = states[1];
+    q2 = states[2];
+    q3 = states[3];
+    dax_b = states[10];
+    day_b = states[11];
+    daz_b = states[12];
+    daxCov = sq(dt*1.4544411e-2f);
+    dayCov = sq(dt*1.4544411e-2f);
+    dazCov = sq(dt*1.4544411e-2f);
+    if (onGround) dazCov = dazCov * sq(yawVarScale);
+    dvxCov = sq(dt*0.5f);
+    dvyCov = sq(dt*0.5f);
+    dvzCov = sq(dt*0.5f);
+
+    // Predicted covariance calculation
+    SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3;
+    SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
+    SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
+    SF[3] = day/2 - day_b/2;
+    SF[4] = daz/2 - daz_b/2;
+    SF[5] = dax/2 - dax_b/2;
+    SF[6] = dax_b/2 - dax/2;
+    SF[7] = daz_b/2 - daz/2;
+    SF[8] = day_b/2 - day/2;
+    SF[9] = q1/2;
+    SF[10] = q2/2;
+    SF[11] = q3/2;
+    SF[12] = 2*dvz*q0;
+    SF[13] = 2*dvy*q1;
+
+    SG[0] = q0/2;
+    SG[1] = sq(q3);
+    SG[2] = sq(q2);
+    SG[3] = sq(q1);
+    SG[4] = sq(q0);
+    SG[5] = 2*q2*q3;
+    SG[6] = 2*q1*q3;
+    SG[7] = 2*q1*q2;
+
+    SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
+    SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
+    SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
+    SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4;
+    SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4;
+    SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4;
+    SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4;
+    SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2;
+    SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4;
+    SQ[9] = sq(SG[0]);
+    SQ[10] = sq(q1);
+
+    SPP[0] = SF[12] + SF[13] - 2*dvx*q2;
+    SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
+    SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
+    SPP[3] = SF[11];
+    SPP[4] = SF[10];
+    SPP[5] = SF[9];
+    SPP[6] = SF[7];
+    SPP[7] = SF[8];
+
+    nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
+    nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2;
+    nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2;
+    nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2;
+    nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]);
+    nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]);
+    nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]);
+    nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]);
+    nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]);
+    nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]);
+    nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3];
+    nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3];
+    nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3];
+    nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3];
+    nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3];
+    nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3];
+    nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3];
+    nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3];
+    nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3];
+    nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3];
+    nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3];
+    nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2);
+    nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2;
+    nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2;
+    nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2;
+    nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2);
+    nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2);
+    nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2);
+    nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2);
+    nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2);
+    nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2);
+    nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2;
+    nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2;
+    nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2;
+    nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2;
+    nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2;
+    nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2;
+    nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2;
+    nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2;
+    nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2;
+    nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2;
+    nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2;
+    nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2);
+    nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2;
+    nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2;
+    nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2;
+    nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2);
+    nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2);
+    nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2);
+    nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2);
+    nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2);
+    nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2);
+    nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2;
+    nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2;
+    nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2;
+    nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2;
+    nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2;
+    nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2;
+    nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2;
+    nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2;
+    nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2;
+    nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2;
+    nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2;
+    nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2);
+    nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2;
+    nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2;
+    nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2;
+    nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2);
+    nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2);
+    nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2);
+    nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2);
+    nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2);
+    nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2);
+    nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2;
+    nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2;
+    nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2;
+    nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2;
+    nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2;
+    nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2;
+    nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2;
+    nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2;
+    nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2;
+    nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2;
+    nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2;
+    nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]);
+    nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2;
+    nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2;
+    nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2;
+    nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]);
+    nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]);
+    nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]);
+    nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]);
+    nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]);
+    nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]);
+    nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2];
+    nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2];
+    nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2];
+    nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2];
+    nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2];
+    nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2];
+    nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2];
+    nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2];
+    nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2];
+    nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2];
+    nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2];
+    nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]);
+    nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2;
+    nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2;
+    nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2;
+    nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]);
+    nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]);
+    nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]);
+    nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]);
+    nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]);
+    nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]);
+    nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0];
+    nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0];
+    nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0];
+    nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0];
+    nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0];
+    nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0];
+    nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0];
+    nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0];
+    nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0];
+    nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0];
+    nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0];
+    nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]);
+    nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2;
+    nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2;
+    nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2;
+    nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]);
+    nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]);
+    nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]);
+    nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]);
+    nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]);
+    nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]);
+    nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1];
+    nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1];
+    nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1];
+    nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1];
+    nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1];
+    nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1];
+    nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1];
+    nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1];
+    nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1];
+    nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1];
+    nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1];
+    nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt);
+    nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2;
+    nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
+    nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2;
+    nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt);
+    nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt);
+    nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt);
+    nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
+    nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
+    nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
+    nextP[7][10] = P[7][10] + P[4][10]*dt;
+    nextP[7][11] = P[7][11] + P[4][11]*dt;
+    nextP[7][12] = P[7][12] + P[4][12]*dt;
+    nextP[7][13] = P[7][13] + P[4][13]*dt;
+    nextP[7][14] = P[7][14] + P[4][14]*dt;
+    nextP[7][15] = P[7][15] + P[4][15]*dt;
+    nextP[7][16] = P[7][16] + P[4][16]*dt;
+    nextP[7][17] = P[7][17] + P[4][17]*dt;
+    nextP[7][18] = P[7][18] + P[4][18]*dt;
+    nextP[7][19] = P[7][19] + P[4][19]*dt;
+    nextP[7][20] = P[7][20] + P[4][20]*dt;
+    nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt);
+    nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2;
+    nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
+    nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2;
+    nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt);
+    nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt);
+    nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt);
+    nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt);
+    nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
+    nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
+    nextP[8][10] = P[8][10] + P[5][10]*dt;
+    nextP[8][11] = P[8][11] + P[5][11]*dt;
+    nextP[8][12] = P[8][12] + P[5][12]*dt;
+    nextP[8][13] = P[8][13] + P[5][13]*dt;
+    nextP[8][14] = P[8][14] + P[5][14]*dt;
+    nextP[8][15] = P[8][15] + P[5][15]*dt;
+    nextP[8][16] = P[8][16] + P[5][16]*dt;
+    nextP[8][17] = P[8][17] + P[5][17]*dt;
+    nextP[8][18] = P[8][18] + P[5][18]*dt;
+    nextP[8][19] = P[8][19] + P[5][19]*dt;
+    nextP[8][20] = P[8][20] + P[5][20]*dt;
+    nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt);
+    nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2;
+    nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
+    nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2;
+    nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt);
+    nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt);
+    nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt);
+    nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt);
+    nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt);
+    nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
+    nextP[9][10] = P[9][10] + P[6][10]*dt;
+    nextP[9][11] = P[9][11] + P[6][11]*dt;
+    nextP[9][12] = P[9][12] + P[6][12]*dt;
+    nextP[9][13] = P[9][13] + P[6][13]*dt;
+    nextP[9][14] = P[9][14] + P[6][14]*dt;
+    nextP[9][15] = P[9][15] + P[6][15]*dt;
+    nextP[9][16] = P[9][16] + P[6][16]*dt;
+    nextP[9][17] = P[9][17] + P[6][17]*dt;
+    nextP[9][18] = P[9][18] + P[6][18]*dt;
+    nextP[9][19] = P[9][19] + P[6][19]*dt;
+    nextP[9][20] = P[9][20] + P[6][20]*dt;
+    nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3];
+    nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2;
+    nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2;
+    nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2;
+    nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2];
+    nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0];
+    nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1];
+    nextP[10][7] = P[10][7] + P[10][4]*dt;
+    nextP[10][8] = P[10][8] + P[10][5]*dt;
+    nextP[10][9] = P[10][9] + P[10][6]*dt;
+    nextP[10][10] = P[10][10];
+    nextP[10][11] = P[10][11];
+    nextP[10][12] = P[10][12];
+    nextP[10][13] = P[10][13];
+    nextP[10][14] = P[10][14];
+    nextP[10][15] = P[10][15];
+    nextP[10][16] = P[10][16];
+    nextP[10][17] = P[10][17];
+    nextP[10][18] = P[10][18];
+    nextP[10][19] = P[10][19];
+    nextP[10][20] = P[10][20];
+    nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3];
+    nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2;
+    nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2;
+    nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2;
+    nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2];
+    nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0];
+    nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1];
+    nextP[11][7] = P[11][7] + P[11][4]*dt;
+    nextP[11][8] = P[11][8] + P[11][5]*dt;
+    nextP[11][9] = P[11][9] + P[11][6]*dt;
+    nextP[11][10] = P[11][10];
+    nextP[11][11] = P[11][11];
+    nextP[11][12] = P[11][12];
+    nextP[11][13] = P[11][13];
+    nextP[11][14] = P[11][14];
+    nextP[11][15] = P[11][15];
+    nextP[11][16] = P[11][16];
+    nextP[11][17] = P[11][17];
+    nextP[11][18] = P[11][18];
+    nextP[11][19] = P[11][19];
+    nextP[11][20] = P[11][20];
+    nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3];
+    nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2;
+    nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2;
+    nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2;
+    nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2];
+    nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0];
+    nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1];
+    nextP[12][7] = P[12][7] + P[12][4]*dt;
+    nextP[12][8] = P[12][8] + P[12][5]*dt;
+    nextP[12][9] = P[12][9] + P[12][6]*dt;
+    nextP[12][10] = P[12][10];
+    nextP[12][11] = P[12][11];
+    nextP[12][12] = P[12][12];
+    nextP[12][13] = P[12][13];
+    nextP[12][14] = P[12][14];
+    nextP[12][15] = P[12][15];
+    nextP[12][16] = P[12][16];
+    nextP[12][17] = P[12][17];
+    nextP[12][18] = P[12][18];
+    nextP[12][19] = P[12][19];
+    nextP[12][20] = P[12][20];
+    nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3];
+    nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2;
+    nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2;
+    nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2;
+    nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2];
+    nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0];
+    nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1];
+    nextP[13][7] = P[13][7] + P[13][4]*dt;
+    nextP[13][8] = P[13][8] + P[13][5]*dt;
+    nextP[13][9] = P[13][9] + P[13][6]*dt;
+    nextP[13][10] = P[13][10];
+    nextP[13][11] = P[13][11];
+    nextP[13][12] = P[13][12];
+    nextP[13][13] = P[13][13];
+    nextP[13][14] = P[13][14];
+    nextP[13][15] = P[13][15];
+    nextP[13][16] = P[13][16];
+    nextP[13][17] = P[13][17];
+    nextP[13][18] = P[13][18];
+    nextP[13][19] = P[13][19];
+    nextP[13][20] = P[13][20];
+    nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3];
+    nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2;
+    nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2;
+    nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2;
+    nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2];
+    nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0];
+    nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1];
+    nextP[14][7] = P[14][7] + P[14][4]*dt;
+    nextP[14][8] = P[14][8] + P[14][5]*dt;
+    nextP[14][9] = P[14][9] + P[14][6]*dt;
+    nextP[14][10] = P[14][10];
+    nextP[14][11] = P[14][11];
+    nextP[14][12] = P[14][12];
+    nextP[14][13] = P[14][13];
+    nextP[14][14] = P[14][14];
+    nextP[14][15] = P[14][15];
+    nextP[14][16] = P[14][16];
+    nextP[14][17] = P[14][17];
+    nextP[14][18] = P[14][18];
+    nextP[14][19] = P[14][19];
+    nextP[14][20] = P[14][20];
+    nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3];
+    nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2;
+    nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2;
+    nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2;
+    nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2];
+    nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0];
+    nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1];
+    nextP[15][7] = P[15][7] + P[15][4]*dt;
+    nextP[15][8] = P[15][8] + P[15][5]*dt;
+    nextP[15][9] = P[15][9] + P[15][6]*dt;
+    nextP[15][10] = P[15][10];
+    nextP[15][11] = P[15][11];
+    nextP[15][12] = P[15][12];
+    nextP[15][13] = P[15][13];
+    nextP[15][14] = P[15][14];
+    nextP[15][15] = P[15][15];
+    nextP[15][16] = P[15][16];
+    nextP[15][17] = P[15][17];
+    nextP[15][18] = P[15][18];
+    nextP[15][19] = P[15][19];
+    nextP[15][20] = P[15][20];
+    nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3];
+    nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2;
+    nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2;
+    nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2;
+    nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2];
+    nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0];
+    nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1];
+    nextP[16][7] = P[16][7] + P[16][4]*dt;
+    nextP[16][8] = P[16][8] + P[16][5]*dt;
+    nextP[16][9] = P[16][9] + P[16][6]*dt;
+    nextP[16][10] = P[16][10];
+    nextP[16][11] = P[16][11];
+    nextP[16][12] = P[16][12];
+    nextP[16][13] = P[16][13];
+    nextP[16][14] = P[16][14];
+    nextP[16][15] = P[16][15];
+    nextP[16][16] = P[16][16];
+    nextP[16][17] = P[16][17];
+    nextP[16][18] = P[16][18];
+    nextP[16][19] = P[16][19];
+    nextP[16][20] = P[16][20];
+    nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3];
+    nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2;
+    nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2;
+    nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2;
+    nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2];
+    nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0];
+    nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1];
+    nextP[17][7] = P[17][7] + P[17][4]*dt;
+    nextP[17][8] = P[17][8] + P[17][5]*dt;
+    nextP[17][9] = P[17][9] + P[17][6]*dt;
+    nextP[17][10] = P[17][10];
+    nextP[17][11] = P[17][11];
+    nextP[17][12] = P[17][12];
+    nextP[17][13] = P[17][13];
+    nextP[17][14] = P[17][14];
+    nextP[17][15] = P[17][15];
+    nextP[17][16] = P[17][16];
+    nextP[17][17] = P[17][17];
+    nextP[17][18] = P[17][18];
+    nextP[17][19] = P[17][19];
+    nextP[17][20] = P[17][20];
+    nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3];
+    nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2;
+    nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2;
+    nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2;
+    nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2];
+    nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0];
+    nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1];
+    nextP[18][7] = P[18][7] + P[18][4]*dt;
+    nextP[18][8] = P[18][8] + P[18][5]*dt;
+    nextP[18][9] = P[18][9] + P[18][6]*dt;
+    nextP[18][10] = P[18][10];
+    nextP[18][11] = P[18][11];
+    nextP[18][12] = P[18][12];
+    nextP[18][13] = P[18][13];
+    nextP[18][14] = P[18][14];
+    nextP[18][15] = P[18][15];
+    nextP[18][16] = P[18][16];
+    nextP[18][17] = P[18][17];
+    nextP[18][18] = P[18][18];
+    nextP[18][19] = P[18][19];
+    nextP[18][20] = P[18][20];
+    nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3];
+    nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2;
+    nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2;
+    nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2;
+    nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2];
+    nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0];
+    nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1];
+    nextP[19][7] = P[19][7] + P[19][4]*dt;
+    nextP[19][8] = P[19][8] + P[19][5]*dt;
+    nextP[19][9] = P[19][9] + P[19][6]*dt;
+    nextP[19][10] = P[19][10];
+    nextP[19][11] = P[19][11];
+    nextP[19][12] = P[19][12];
+    nextP[19][13] = P[19][13];
+    nextP[19][14] = P[19][14];
+    nextP[19][15] = P[19][15];
+    nextP[19][16] = P[19][16];
+    nextP[19][17] = P[19][17];
+    nextP[19][18] = P[19][18];
+    nextP[19][19] = P[19][19];
+    nextP[19][20] = P[19][20];
+    nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3];
+    nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2;
+    nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2;
+    nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2;
+    nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2];
+    nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0];
+    nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1];
+    nextP[20][7] = P[20][7] + P[20][4]*dt;
+    nextP[20][8] = P[20][8] + P[20][5]*dt;
+    nextP[20][9] = P[20][9] + P[20][6]*dt;
+    nextP[20][10] = P[20][10];
+    nextP[20][11] = P[20][11];
+    nextP[20][12] = P[20][12];
+    nextP[20][13] = P[20][13];
+    nextP[20][14] = P[20][14];
+    nextP[20][15] = P[20][15];
+    nextP[20][16] = P[20][16];
+    nextP[20][17] = P[20][17];
+    nextP[20][18] = P[20][18];
+    nextP[20][19] = P[20][19];
+    nextP[20][20] = P[20][20];
+
+    for (unsigned i = 0; i < n_states; i++)
+    {
+        nextP[i][i] = nextP[i][i] + processNoise[i];
+    }
+
+    // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by
+    // setting the coresponding covariance terms to zero.
+    if (onGround || !useCompass)
+    {
+        zeroRows(nextP,15,20);
+        zeroCols(nextP,15,20);
+    }
+
+    // If on ground or not using airspeed sensing, inhibit wind velocity
+    // covariance growth.
+    if (onGround || !useAirspeed)
+    {
+        zeroRows(nextP,13,14);
+        zeroCols(nextP,13,14);
+    }
+
+    // If the total position variance exceds 1E6 (1000m), then stop covariance
+    // growth by setting the predicted to the previous values
+    // This prevent an ill conditioned matrix from occurring for long periods
+    // without GPS
+    if ((P[7][7] + P[8][8]) > 1E6f)
+    {
+        for (uint8_t i=7; i<=8; i++)
+        {
+            for (unsigned j = 0; j < n_states; j++)
+            {
+                nextP[i][j] = P[i][j];
+                nextP[j][i] = P[j][i];
+            }
+        }
+    }
+
+    if (onGround || staticMode) {
+        // copy the portion of the variances we want to
+        // propagate
+        for (unsigned i = 0; i < 14; i++) {
+            P[i][i] = nextP[i][i];
+
+            // force symmetry for observable states
+            // force zero for non-observable states
+            for (unsigned i = 1; i < n_states; i++)
+            {
+                for (uint8_t j = 0; j < i; j++)
+                {
+                    if ((i > 12) || (j > 12)) {
+                        P[i][j] = 0.0f;
+                    } else {
+                        P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+                    }
+                    P[j][i] = P[i][j];
+                }
+            }
+        }
+
+    } else {
+
+        // Copy covariance
+        for (unsigned i = 0; i < n_states; i++) {
+            P[i][i] = nextP[i][i];
+        }
+
+        // force symmetry for observable states
+        for (unsigned i = 1; i < n_states; i++)
+        {
+            for (uint8_t j = 0; j < i; j++)
+            {
+                P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+                P[j][i] = P[i][j];
+            }
+        }
+
+    }
+
+    ConstrainVariances();
+}
+
+void AttPosEKF::FuseVelposNED()
+{
+
+// declare variables used by fault isolation logic
+    uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
+    uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
+    uint32_t hgtRetryTime = 5000; // height measurement retry time
+    uint32_t horizRetryTime;
+
+// declare variables used to check measurement errors
+    float velInnov[3] = {0.0f,0.0f,0.0f};
+    float posInnov[2] = {0.0f,0.0f};
+    float hgtInnov = 0.0f;
+
+// declare variables used to control access to arrays
+    bool fuseData[6] = {false,false,false,false,false,false};
+    uint8_t stateIndex;
+    uint8_t obsIndex;
+    uint8_t indexLimit;
+
+// declare variables used by state and covariance update calculations
+    float velErr;
+    float posErr;
+    float R_OBS[6];
+    float observation[6];
+    float SK;
+    float quatMag;
+
+// Perform sequential fusion of GPS measurements. This assumes that the
+// errors in the different velocity and position components are
+// uncorrelated which is not true, however in the absence of covariance
+// data from the GPS receiver it is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+    if (fuseVelData || fusePosData || fuseHgtData)
+    {
+        // set the GPS data timeout depending on whether airspeed data is present
+        if (useAirspeed) horizRetryTime = gpsRetryTime;
+        else horizRetryTime = gpsRetryTimeNoTAS;
+
+        // Form the observation vector
+        for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
+        for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
+        observation[5] = -(hgtMea);
+
+        // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
+        velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
+        posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
+        R_OBS[0] = 0.04f + sq(velErr);
+        R_OBS[1] = R_OBS[0];
+        R_OBS[2] = 0.08f + sq(velErr);
+        R_OBS[3] = R_OBS[2];
+        R_OBS[4] = 4.0f  + sq(posErr);
+        R_OBS[5] = 4.0f;
+
+        // Set innovation variances to zero default
+        for (uint8_t i = 0; i<=5; i++)
+        {
+            varInnovVelPos[i] = 0.0f;
+        }
+        // calculate innovations and check GPS data validity using an innovation consistency check
+        if (fuseVelData)
+        {
+            // test velocity measurements
+            uint8_t imax = 2;
+            if (fusionModeGPS == 1) imax = 1;
+            for (uint8_t i = 0; i<=imax; i++)
+            {
+                velInnov[i] = statesAtVelTime[i+4] - velNED[i];
+                stateIndex = 4 + i;
+                varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
+            }
+            // apply a 5-sigma threshold
+            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
+            current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
+            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
+            {
+                current_ekf_state.velHealth = true;
+                current_ekf_state.velFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.velHealth = false;
+            }
+        }
+        if (fusePosData)
+        {
+            // test horizontal position measurements
+            posInnov[0] = statesAtPosTime[7] - posNE[0];
+            posInnov[1] = statesAtPosTime[8] - posNE[1];
+            varInnovVelPos[3] = P[7][7] + R_OBS[3];
+            varInnovVelPos[4] = P[8][8] + R_OBS[4];
+            // apply a 10-sigma threshold
+            current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
+            current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
+            if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
+            {
+                current_ekf_state.posHealth = true;
+                current_ekf_state.posFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.posHealth = false;
+            }
+        }
+        // test height measurements
+        if (fuseHgtData)
+        {
+            hgtInnov = statesAtHgtTime[9] + hgtMea;
+            varInnovVelPos[5] = P[9][9] + R_OBS[5];
+            // apply a 10-sigma threshold
+            current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
+            current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
+            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
+            {
+                current_ekf_state.hgtHealth = true;
+                current_ekf_state.hgtFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.hgtHealth = false;
+            }
+        }
+        // Set range for sequential fusion of velocity and position measurements depending
+        // on which data is available and its health
+        if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth)
+        {
+            fuseData[0] = true;
+            fuseData[1] = true;
+            fuseData[2] = true;
+        }
+        if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth)
+        {
+            fuseData[0] = true;
+            fuseData[1] = true;
+        }
+        if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth)
+        {
+            fuseData[3] = true;
+            fuseData[4] = true;
+        }
+        if (fuseHgtData && current_ekf_state.hgtHealth)
+        {
+            fuseData[5] = true;
+        }
+        // Limit range of states modified when on ground
+        if(!onGround)
+        {
+            indexLimit = 20;
+        }
+        else
+        {
+            indexLimit = 12;
+        }
+        // Fuse measurements sequentially
+        for (obsIndex=0; obsIndex<=5; obsIndex++)
+        {
+            if (fuseData[obsIndex])
+            {
+                stateIndex = 4 + obsIndex;
+                // Calculate the measurement innovation, using states from a
+                // different time coordinate if fusing height data
+                if (obsIndex >= 0 && obsIndex <= 2)
+                {
+                    innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
+                }
+                else if (obsIndex == 3 || obsIndex == 4)
+                {
+                    innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
+                }
+                else if (obsIndex == 5)
+                {
+                    innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
+                }
+                // Calculate the Kalman Gain
+                // Calculate innovation variances - also used for data logging
+                varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
+                SK = 1.0/varInnovVelPos[obsIndex];
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    Kfusion[i] = P[i][stateIndex]*SK;
+                }
+                // Calculate state corrections and re-normalise the quaternions
+                for (uint8_t i = 0; i<=indexLimit; i++)
+                {
+                    states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
+                }
+                quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+                if (quatMag > 1e-12f) // divide by  0 protection
+                {
+                    for (uint8_t i = 0; i<=3; i++)
+                    {
+                        states[i] = states[i] / quatMag;
+                    }
+                }
+                // Update the covariance - take advantage of direct observation of a
+                // single state at index = stateIndex to reduce computations
+                // Optimised implementation of standard equation P = (I - K*H)*P;
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    for (uint8_t j= 0; j<=indexLimit; j++)
+                    {
+                        KHP[i][j] = Kfusion[i] * P[stateIndex][j];
+                    }
+                }
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    for (uint8_t j= 0; j<=indexLimit; j++)
+                    {
+                        P[i][j] = P[i][j] - KHP[i][j];
+                    }
+                }
+            }
+        }
+    }
+
+    ForceSymmetry();
+    ConstrainVariances();
+
+    //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
+}
+
+void AttPosEKF::FuseMagnetometer()
+{
+    uint8_t obsIndex;
+    uint8_t indexLimit;
+    float DCM[3][3] =
+    {
+        {1.0f,0.0f,0.0f} ,
+        {0.0f,1.0f,0.0f} ,
+        {0.0f,0.0f,1.0f}
+    };
+    float MagPred[3] = {0.0f,0.0f,0.0f};
+    float SK_MX[6];
+    float SK_MY[5];
+    float SK_MZ[6];
+    float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
+
+// Perform sequential fusion of Magnetometer measurements.
+// This assumes that the errors in the different components are
+// uncorrelated which is not true, however in the absence of covariance
+// data fit is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+    if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
+    {
+        // Limit range of states modified when on ground
+        if(!onGround)
+        {
+            indexLimit = 20;
+        }
+        else
+        {
+            indexLimit = 12;
+        }
+
+        static float q0 = 0.0f;
+        static float q1 = 0.0f;
+        static float q2 = 0.0f;
+        static float q3 = 1.0f;
+        static float magN = 0.4f;
+        static float magE = 0.0f;
+        static float magD = 0.3f;
+
+        static float R_MAG = 0.0025f;
+
+        float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
+
+        // Sequential fusion of XYZ components to spread processing load across
+        // three prediction time steps.
+
+        // Calculate observation jacobians and Kalman gains
+        if (fuseMagData)
+        {
+            static float magXbias = 0.0f;
+            static float magYbias = 0.0f;
+            static float magZbias = 0.0f;
+
+            // Copy required states to local variable names
+            q0       = statesAtMagMeasTime[0];
+            q1       = statesAtMagMeasTime[1];
+            q2       = statesAtMagMeasTime[2];
+            q3       = statesAtMagMeasTime[3];
+            magN     = statesAtMagMeasTime[15];
+            magE     = statesAtMagMeasTime[16];
+            magD     = statesAtMagMeasTime[17];
+            magXbias = statesAtMagMeasTime[18];
+            magYbias = statesAtMagMeasTime[19];
+            magZbias = statesAtMagMeasTime[20];
+
+            // rotate predicted earth components into body axes and calculate
+            // predicted measurments
+            DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+            DCM[0][1] = 2*(q1*q2 + q0*q3);
+            DCM[0][2] = 2*(q1*q3-q0*q2);
+            DCM[1][0] = 2*(q1*q2 - q0*q3);
+            DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
+            DCM[1][2] = 2*(q2*q3 + q0*q1);
+            DCM[2][0] = 2*(q1*q3 + q0*q2);
+            DCM[2][1] = 2*(q2*q3 - q0*q1);
+            DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+            MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE  + DCM[0][2]*magD + magXbias;
+            MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE  + DCM[1][2]*magD + magYbias;
+            MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE  + DCM[2][2]*magD + magZbias;
+
+            // scale magnetometer observation error with total angular rate
+            R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU);
+
+            // Calculate observation jacobians
+            SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
+            SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+            SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+            SH_MAG[3] = sq(q3);
+            SH_MAG[4] = sq(q2);
+            SH_MAG[5] = sq(q1);
+            SH_MAG[6] = sq(q0);
+            SH_MAG[7] = 2*magN*q0;
+            SH_MAG[8] = 2*magE*q3;
+
+            for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            H_MAG[1] = SH_MAG[0];
+            H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
+            H_MAG[3] = SH_MAG[2];
+            H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
+            H_MAG[16] = 2*q0*q3 + 2*q1*q2;
+            H_MAG[17] = 2*q1*q3 - 2*q0*q2;
+            H_MAG[18] = 1.0f;
+
+            // Calculate Kalman gain
+            SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
+            SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+            SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MX[4] = 2*q0*q2 - 2*q1*q3;
+            SK_MX[5] = 2*q0*q3 + 2*q1*q2;
+            Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]);
+            Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]);
+            Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]);
+            Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]);
+            Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]);
+            Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]);
+            Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]);
+            Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]);
+            Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]);
+            Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]);
+            Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]);
+            Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]);
+            Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]);
+            Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]);
+            Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]);
+            Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]);
+            Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]);
+            Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]);
+            Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]);
+            Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]);
+            Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]);
+            varInnovMag[0] = 1.0f/SK_MX[0];
+            innovMag[0] = MagPred[0] - magData.x;
+
+            // reset the observation index to 0 (we start by fusing the X
+            // measurement)
+            obsIndex = 0;
+        }
+        else if (obsIndex == 1) // we are now fusing the Y measurement
+        {
+            // Calculate observation jacobians
+            for (unsigned int i=0; i<n_states; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[2];
+            H_MAG[1] = SH_MAG[1];
+            H_MAG[2] = SH_MAG[0];
+            H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
+            H_MAG[15] = 2*q1*q2 - 2*q0*q3;
+            H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[17] = 2*q0*q1 + 2*q2*q3;
+            H_MAG[19] = 1;
+
+            // Calculate Kalman gain
+            SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
+            SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MY[3] = 2*q0*q3 - 2*q1*q2;
+            SK_MY[4] = 2*q0*q1 + 2*q2*q3;
+            Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]);
+            Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]);
+            Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]);
+            Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]);
+            Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]);
+            Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]);
+            Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]);
+            Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]);
+            Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]);
+            Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]);
+            Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]);
+            Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]);
+            Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]);
+            Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]);
+            Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]);
+            Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]);
+            Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]);
+            Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]);
+            Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]);
+            Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]);
+            Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]);
+            varInnovMag[1] = 1.0f/SK_MY[0];
+            innovMag[1] = MagPred[1] - magData.y;
+        }
+        else if (obsIndex == 2) // we are now fusing the Z measurement
+        {
+            // Calculate observation jacobians
+            for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[1];
+            H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
+            H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            H_MAG[3] = SH_MAG[0];
+            H_MAG[15] = 2*q0*q2 + 2*q1*q3;
+            H_MAG[16] = 2*q2*q3 - 2*q0*q1;
+            H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[20] = 1;
+
+            // Calculate Kalman gain
+            SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+            SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
+            SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
+            Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]);
+            Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]);
+            Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]);
+            Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]);
+            Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]);
+            Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]);
+            Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]);
+            Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]);
+            Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]);
+            Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]);
+            Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]);
+            Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]);
+            Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]);
+            Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]);
+            Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]);
+            Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]);
+            Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]);
+            Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]);
+            Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]);
+            Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]);
+            Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]);
+            varInnovMag[2] = 1.0f/SK_MZ[0];
+            innovMag[2] = MagPred[2] - magData.z;
+
+        }
+
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
+        {
+            // correct the state vector
+            for (uint8_t j= 0; j<=indexLimit; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12)
+            {
+                for (uint8_t j= 0; j<=3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in KH to reduce the
+            // number of operations
+            for (uint8_t i = 0; i<=indexLimit; i++)
+            {
+                for (uint8_t j = 0; j<=3; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_MAG[j];
+                }
+                for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f;
+                if (!onGround)
+                {
+                    for (uint8_t j = 15; j<=20; j++)
+                    {
+                        KH[i][j] = Kfusion[i] * H_MAG[j];
+                    }
+                }
+                else
+                {
+                    for (uint8_t j = 15; j<=20; j++)
+                    {
+                        KH[i][j] = 0.0f;
+                    }
+                }
+            }
+            for (uint8_t i = 0; i<=indexLimit; i++)
+            {
+                for (uint8_t j = 0; j<=indexLimit; j++)
+                {
+                    KHP[i][j] = 0.0f;
+                    for (uint8_t k = 0; k<=3; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    if (!onGround)
+                    {
+                        for (uint8_t k = 15; k<=20; k++)
+                        {
+                            KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                        }
+                    }
+                }
+            }
+        }
+        for (uint8_t i = 0; i<=indexLimit; i++)
+        {
+            for (uint8_t j = 0; j<=indexLimit; j++)
+            {
+                P[i][j] = P[i][j] - KHP[i][j];
+            }
+        }
+    }
+    obsIndex = obsIndex + 1;
+
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::FuseAirspeed()
+{
+    float vn;
+    float ve;
+    float vd;
+    float vwn;
+    float vwe;
+    const float R_TAS = 2.0f;
+    float SH_TAS[3];
+    float Kfusion[21];
+    float VtasPred;
+
+    // Copy required states to local variable names
+    vn = statesAtVtasMeasTime[4];
+    ve = statesAtVtasMeasTime[5];
+    vd = statesAtVtasMeasTime[6];
+    vwn = statesAtVtasMeasTime[13];
+    vwe = statesAtVtasMeasTime[14];
+
+    // Need to check that it is flying before fusing airspeed data
+    // Calculate the predicted airspeed
+    VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
+    // Perform fusion of True Airspeed measurement
+    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
+    {
+        // Calculate observation jacobians
+        SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
+        SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
+        SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
+        
+        float H_TAS[21];
+        for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
+        H_TAS[4] = SH_TAS[2];
+        H_TAS[5] = SH_TAS[1];
+        H_TAS[6] = vd*SH_TAS[0];
+        H_TAS[13] = -SH_TAS[2];
+        H_TAS[14] = -SH_TAS[1];
+
+        // Calculate Kalman gains
+        float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
+        Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
+        Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
+        Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
+        Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
+        Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
+        Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
+        Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
+        Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
+        Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
+        Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
+        Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
+        Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
+        Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
+        Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
+        Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
+        Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
+        Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
+        Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
+        Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
+        Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
+        Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
+        varInnovVtas = 1.0f/SK_TAS;
+
+        // Calculate the measurement innovation
+        innovVtas = VtasPred - VtasMeas;
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovVtas*innovVtas*SK_TAS) < 25.0)
+        {
+            // correct the state vector
+            for (uint8_t j=0; j<=20; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovVtas;
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12f)
+            {
+                for (uint8_t j= 0; j<=3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in H to reduce the
+            // number of operations
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 4; j<=6; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 13; j<=14; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0;
+            }
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=20; j++)
+                {
+                    KHP[i][j] = 0.0;
+                    for (uint8_t k = 4; k<=6; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    for (uint8_t k = 13; k<=14; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                }
+            }
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=20; j++)
+                {
+                    P[i][j] = P[i][j] - KHP[i][j];
+                }
+            }
+        }
+    }
+
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+    uint8_t row;
+    uint8_t col;
+    for (row=first; row<=last; row++)
+    {
+        for (col=0; col<n_states; col++)
+        {
+            covMat[row][col] = 0.0;
+        }
+    }
+}
+
+void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+    uint8_t row;
+    uint8_t col;
+    for (col=first; col<=last; col++)
+    {
+        for (row=0; row < n_states; row++)
+        {
+            covMat[row][col] = 0.0;
+        }
+    }
+}
+
+float AttPosEKF::sq(float valIn)
+{
+    return valIn*valIn;
+}
+
+// Store states in a history array along with time stamp
+void AttPosEKF::StoreStates(uint64_t timestamp_ms)
+{
+    for (unsigned i=0; i<n_states; i++)
+        storedStates[i][storeIndex] = states[i];
+    statetimeStamp[storeIndex] = timestamp_ms;
+    storeIndex++;
+    if (storeIndex == data_buffer_size)
+        storeIndex = 0;
+}
+
+void AttPosEKF::ResetStoredStates()
+{
+    // reset all stored states
+    memset(&storedStates[0][0], 0, sizeof(storedStates));
+    memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
+
+    // reset store index to first
+    storeIndex = 0;
+
+    // overwrite all existing states
+    for (unsigned i = 0; i < n_states; i++) {
+        storedStates[i][storeIndex] = states[i];
+    }
+
+    statetimeStamp[storeIndex] = millis();
+
+    // increment to next storage index
+    storeIndex++;
+}
+
+// Output the state vector stored at the time that best matches that specified by msec
+int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
+{
+    int ret = 0;
+
+    // int64_t bestTimeDelta = 200;
+    // unsigned bestStoreIndex = 0;
+    // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
+    // {
+    //     // The time delta can also end up as negative number,
+    //     // since we might compare future to past or past to future
+    //     // therefore cast to int64.
+    //     int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex];
+    //     if (timeDelta < 0) {
+    //         timeDelta = -timeDelta;
+    //     }
+
+    //     if (timeDelta < bestTimeDelta)
+    //     {
+    //         bestStoreIndex = storeIndex;
+    //         bestTimeDelta = timeDelta;
+    //     }
+    // }
+    // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
+    // {
+    //     for (uint8_t i=0; i < n_states; i++) {
+    //         if (isfinite(storedStates[i][bestStoreIndex])) {
+    //             statesForFusion[i] = storedStates[i][bestStoreIndex];
+    //         } else if (isfinite(states[i])) {
+    //             statesForFusion[i] = states[i];
+    //         } else {
+    //             // There is not much we can do here, except reporting the error we just
+    //             // found.
+    //             ret++;
+    //         }
+    // }
+    // else // otherwise output current state
+    // {
+        for (uint8_t i=0; i < n_states; i++) {
+            if (isfinite(states[i])) {
+                statesForFusion[i] = states[i];
+            } else {
+                ret++;
+            }
+        }
+    // }
+
+    return ret;
+}
+
+void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
+{
+    // Calculate the nav to body cosine matrix
+    float q00 = sq(quat[0]);
+    float q11 = sq(quat[1]);
+    float q22 = sq(quat[2]);
+    float q33 = sq(quat[3]);
+    float q01 =  quat[0]*quat[1];
+    float q02 =  quat[0]*quat[2];
+    float q03 =  quat[0]*quat[3];
+    float q12 =  quat[1]*quat[2];
+    float q13 =  quat[1]*quat[3];
+    float q23 =  quat[2]*quat[3];
+
+    Tnb.x.x = q00 + q11 - q22 - q33;
+    Tnb.y.y = q00 - q11 + q22 - q33;
+    Tnb.z.z = q00 - q11 - q22 + q33;
+    Tnb.y.x = 2*(q12 - q03);
+    Tnb.z.x = 2*(q13 + q02);
+    Tnb.x.y = 2*(q12 + q03);
+    Tnb.z.y = 2*(q23 - q01);
+    Tnb.x.z = 2*(q13 - q02);
+    Tnb.y.z = 2*(q23 + q01);
+}
+
+void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
+{
+    // Calculate the body to nav cosine matrix
+    float q00 = sq(quat[0]);
+    float q11 = sq(quat[1]);
+    float q22 = sq(quat[2]);
+    float q33 = sq(quat[3]);
+    float q01 =  quat[0]*quat[1];
+    float q02 =  quat[0]*quat[2];
+    float q03 =  quat[0]*quat[3];
+    float q12 =  quat[1]*quat[2];
+    float q13 =  quat[1]*quat[3];
+    float q23 =  quat[2]*quat[3];
+
+    Tbn.x.x = q00 + q11 - q22 - q33;
+    Tbn.y.y = q00 - q11 + q22 - q33;
+    Tbn.z.z = q00 - q11 - q22 + q33;
+    Tbn.x.y = 2*(q12 - q03);
+    Tbn.x.z = 2*(q13 + q02);
+    Tbn.y.x = 2*(q12 + q03);
+    Tbn.y.z = 2*(q23 - q01);
+    Tbn.z.x = 2*(q13 - q02);
+    Tbn.z.y = 2*(q23 + q01);
+}
+
+void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
+{
+    float u1 = cos(0.5f*eul[0]);
+    float u2 = cos(0.5f*eul[1]);
+    float u3 = cos(0.5f*eul[2]);
+    float u4 = sin(0.5f*eul[0]);
+    float u5 = sin(0.5f*eul[1]);
+    float u6 = sin(0.5f*eul[2]);
+    quat[0] = u1*u2*u3+u4*u5*u6;
+    quat[1] = u4*u2*u3-u1*u5*u6;
+    quat[2] = u1*u5*u3+u4*u2*u6;
+    quat[3] = u1*u2*u6-u4*u5*u3;
+}
+
+void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
+{
+    y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
+    y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
+    y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
+}
+
+void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
+{
+    velNED[0] = gpsGndSpd*cosf(gpsCourse);
+    velNED[1] = gpsGndSpd*sinf(gpsCourse);
+    velNED[2] = gpsVelD;
+}
+
+void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+{
+    posNED[0] = earthRadius * (lat - latRef);
+    posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
+    posNED[2] = -(hgt - hgtRef);
+}
+
+void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+{
+    lat = latRef + posNED[0] * earthRadiusInv;
+    lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
+    hgt = hgtRef - posNED[2];
+}
+
+void AttPosEKF::OnGroundCheck()
+{
+    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
+    if (staticMode) {
+        staticMode = !(GPSstatus > GPS_FIX_2D);
+    }
+}
+
+void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
+{
+    //Define Earth rotation vector in the NED navigation frame
+    omega.x  = earthRate*cosf(latitude);
+    omega.y  = 0.0f;
+    omega.z  = -earthRate*sinf(latitude);
+}
+
+void AttPosEKF::CovarianceInit()
+{
+    // Calculate the initial covariance matrix P
+    P[0][0]   = 0.25f * sq(1.0f*deg2rad);
+    P[1][1]   = 0.25f * sq(1.0f*deg2rad);
+    P[2][2]   = 0.25f * sq(1.0f*deg2rad);
+    P[3][3]   = 0.25f * sq(10.0f*deg2rad);
+    P[4][4]   = sq(0.7);
+    P[5][5]   = P[4][4];
+    P[6][6]   = sq(0.7);
+    P[7][7]   = sq(15.0);
+    P[8][8]   = P[7][7];
+    P[9][9]   = sq(5.0);
+    P[10][10] = sq(0.1*deg2rad*dtIMU);
+    P[11][11] = P[10][10];
+    P[12][12] = P[10][10];
+    P[13][13] = sq(8.0f);
+    P[14][4]  = P[13][13];
+    P[15][15] = sq(0.02f);
+    P[16][16] = P[15][15];
+    P[17][17] = P[15][15];
+    P[18][18] = sq(0.02f);
+    P[19][19] = P[18][18];
+    P[20][20] = P[18][18];
+}
+
+float AttPosEKF::ConstrainFloat(float val, float min, float max)
+{
+    return (val > max) ? max : ((val < min) ? min : val);
+}
+
+void AttPosEKF::ConstrainVariances()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // State vector:
+    // 0-3: quaternions (q0, q1, q2, q3)
+    // 4-6: Velocity - m/sec (North, East, Down)
+    // 7-9: Position - m (North, East, Down)
+    // 10-12: Delta Angle bias - rad (X,Y,Z)
+    // 13-14: Wind Vector  - m/sec (North,East)
+    // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+    // Constrain quaternion variances
+    for (unsigned i = 0; i < 4; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Constrain velocitie variances
+    for (unsigned i = 4; i < 7; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Constrain position variances
+    for (unsigned i = 7; i < 10; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
+    }
+
+    // Angle bias variances
+    for (unsigned i = 10; i < 13; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU));
+    }
+
+    // Wind velocity variances
+    for (unsigned i = 13; i < 15; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Earth magnetic field variances
+    for (unsigned i = 15; i < 18; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Body magnetic field variances
+    for (unsigned i = 18; i < 21; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+}
+
+void AttPosEKF::ConstrainStates()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // State vector:
+    // 0-3: quaternions (q0, q1, q2, q3)
+    // 4-6: Velocity - m/sec (North, East, Down)
+    // 7-9: Position - m (North, East, Down)
+    // 10-12: Delta Angle bias - rad (X,Y,Z)
+    // 13-14: Wind Vector  - m/sec (North,East)
+    // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+
+    // Constrain quaternion
+    for (unsigned i = 0; i < 4; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Constrain velocities to what GPS can do for us
+    for (unsigned i = 4; i < 7; i++) {
+        states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f);
+    }
+
+    // Constrain position to a reasonable vehicle range (in meters)
+    for (unsigned i = 7; i < 9; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f);
+    }
+
+    // Constrain altitude
+    states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f);
+
+    // Angle bias limit - set to 8 degrees / sec
+    for (unsigned i = 10; i < 13; i++) {
+        states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
+    }
+
+    // Wind velocity limits - assume 120 m/s max velocity
+    for (unsigned i = 13; i < 15; i++) {
+        states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
+    }
+
+    // Earth magnetic field limits (in Gauss)
+    for (unsigned i = 15; i < 18; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Body magnetic field variances (in Gauss).
+    // the max offset should be in this range.
+    for (unsigned i = 18; i < 21; i++) {
+        states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
+    }
+
+}
+
+void AttPosEKF::ForceSymmetry()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // Force symmetry on the covariance matrix to prevent ill-conditioning
+    // of the matrix which would cause the filter to blow-up
+    for (unsigned i = 1; i < n_states; i++)
+    {
+        for (uint8_t j = 0; j < i; j++)
+        {
+            P[i][j] = 0.5f * (P[i][j] + P[j][i]);
+            P[j][i] = P[i][j];
+        }
+    }
+}
+
+bool AttPosEKF::FilterHealthy()
+{
+    if (!statesInitialised) {
+        return false;
+    }
+
+    // XXX Check state vector for NaNs and ill-conditioning
+
+    // Check if any of the major inputs timed out
+    if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) {
+        return false;
+    }
+
+    // Nothing fired, return ok.
+    return true;
+}
+
+/**
+ * Reset the filter position states
+ *
+ * This resets the position to the last GPS measurement
+ * or to zero in case of static position.
+ */
+void AttPosEKF::ResetPosition(void)
+{
+    if (staticMode) {
+        states[7] = 0;
+        states[8] = 0;
+    } else if (GPSstatus >= GPS_FIX_3D) {
+
+        // reset the states from the GPS measurements
+        states[7] = posNE[0];
+        states[8] = posNE[1];
+    }
+}
+
+/**
+ * Reset the height state.
+ *
+ * This resets the height state with the last altitude measurements
+ */
+void AttPosEKF::ResetHeight(void)
+{
+    // write to the state vector
+    states[9]   = -hgtMea;
+}
+
+/**
+ * Reset the velocity state.
+ */
+void AttPosEKF::ResetVelocity(void)
+{
+    if (staticMode) {
+        states[4] = 0.0f;
+        states[5] = 0.0f;
+        states[6] = 0.0f;
+    } else if (GPSstatus >= GPS_FIX_3D) {
+
+        states[4]  = velNED[0]; // north velocity from last reading
+        states[5]  = velNED[1]; // east velocity from last reading
+        states[6]  = velNED[2]; // down velocity from last reading
+    }
+}
+
+
+void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
+{
+    for (int i = 0; i < n_states; i++)
+    {
+        err->states[i] = states[i];
+    }
+
+    err->velHealth = current_ekf_state.velHealth;
+    err->posHealth = current_ekf_state.posHealth;
+    err->hgtHealth = current_ekf_state.hgtHealth;
+    err->velTimeout = current_ekf_state.velTimeout;
+    err->posTimeout = current_ekf_state.posTimeout;
+    err->hgtTimeout = current_ekf_state.hgtTimeout;
+}
+
+bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
+    bool err = false;
+
+    // check all states and covariance matrices
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            if (!isfinite(KH[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } //  intermediate result used for covariance updates
+            if (!isfinite(KHP[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } // intermediate result used for covariance updates
+            if (!isfinite(P[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } // covariance matrix
+        }
+
+        if (!isfinite(Kfusion[i])) {
+
+            err_report->kalmanGainsNaN = true;
+            err = true;
+        } // Kalman gains
+
+        if (!isfinite(states[i])) {
+
+            err_report->statesNaN = true;
+            err = true;
+        } // state matrix
+    }
+
+    if (err) {
+        FillErrorReport(err_report);
+    }
+
+    return err;
+
+}
+
+/**
+ * Check the filter inputs and bound its operational state
+ *
+ * This check will reset the filter states if required
+ * due to a failure of consistency or timeout checks.
+ * it should be run after the measurement data has been
+ * updated, but before any of the fusion steps are
+ * executed.
+ */
+int AttPosEKF::CheckAndBound()
+{
+
+    // Store the old filter state
+    bool currStaticMode = staticMode;
+
+    // Reset the filter if the states went NaN
+    if (StatesNaN(&last_ekf_error)) {
+
+        InitializeDynamic(velNED, magDeclination);
+
+        return 1;
+    }
+
+    // Reset the filter if the IMU data is too old
+    if (dtIMU > 0.2f) {
+
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        // that's all we can do here, return
+        return 2;
+    }
+
+    // Check if we're on ground - this also sets static mode.
+    OnGroundCheck();
+
+    // Check if we switched between states
+    if (currStaticMode != staticMode) {
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        return 3;
+    }
+
+    return 0;
+}
+
+void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2(-ay, -az);
+    initialPitch = atan2(ax, -az);
+
+    cosRoll = cosf(initialRoll);
+    sinRoll = sinf(initialRoll);
+    cosPitch = cosf(initialPitch);
+    sinPitch = sinf(initialPitch);
+
+    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+    magY = my * cosRoll - mz * sinRoll;
+
+    initialHdg = atan2f(-magY, magX);
+    /* true heading is the mag heading minus declination */
+    initialHdg += declination;
+
+    cosRoll = cosf(initialRoll * 0.5f);
+    sinRoll = sinf(initialRoll * 0.5f);
+
+    cosPitch = cosf(initialPitch * 0.5f);
+    sinPitch = sinf(initialPitch * 0.5f);
+
+    cosHeading = cosf(initialHdg * 0.5f);
+    sinHeading = sinf(initialHdg * 0.5f);
+
+    initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+}
+
+void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
+{
+
+    // Clear the init flag
+    statesInitialised = false;
+
+    magDeclination = declination;
+
+    ZeroVariables();
+
+    // Calculate initial filter quaternion states from raw measurements
+    float initQuat[4];
+    Vector3f initMagXYZ;
+    initMagXYZ   = magData - magBias;
+    AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat);
+
+    // Calculate initial Tbn matrix and rotate Mag measurements into NED
+    // to set initial NED magnetic field states
+    Mat3f DCM;
+    quat2Tbn(DCM, initQuat);
+    Vector3f initMagNED;
+    initMagXYZ   = magData - magBias;
+    initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
+    initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
+    initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
+
+
+
+    // write to state vector
+    for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
+    for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities
+    for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel
+    states[15] = initMagNED.x; // Magnetic Field North
+    states[16] = initMagNED.y; // Magnetic Field East
+    states[17] = initMagNED.z; // Magnetic Field Down
+    states[18] = magBias.x; // Magnetic Field Bias X
+    states[19] = magBias.y; // Magnetic Field Bias Y
+    states[20] = magBias.z; // Magnetic Field Bias Z
+
+    statesInitialised = true;
+
+    // initialise the covariance matrix
+    CovarianceInit();
+
+    //Define Earth rotation vector in the NED navigation frame
+    calcEarthRateNED(earthRateNED, latRef);
+
+    //Initialise summed variables used by covariance prediction
+    summedDelAng.x = 0.0f;
+    summedDelAng.y = 0.0f;
+    summedDelAng.z = 0.0f;
+    summedDelVel.x = 0.0f;
+    summedDelVel.y = 0.0f;
+    summedDelVel.z = 0.0f;
+}
+
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
+{
+    //store initial lat,long and height
+    latRef = referenceLat;
+    lonRef = referenceLon;
+    hgtRef = referenceHgt;
+
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+
+    InitializeDynamic(initvelNED, declination);
+}
+
+void AttPosEKF::ZeroVariables()
+{
+    // Do the data structure init
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            KH[i][j] = 0.0f; //  intermediate result used for covariance updates
+            KHP[i][j] = 0.0f; // intermediate result used for covariance updates
+            P[i][j] = 0.0f; // covariance matrix
+        }
+
+        Kfusion[i] = 0.0f; // Kalman gains
+        states[i] = 0.0f; // state matrix
+    }
+
+    for (unsigned i = 0; i < data_buffer_size; i++) {
+
+        for (unsigned j = 0; j < n_states; j++) {
+            storedStates[j][i] = 0.0f;
+        }
+
+        statetimeStamp[i] = 0;
+    }
+
+    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
+}
+
+void AttPosEKF::GetFilterState(struct ekf_status_report *state)
+{
+    memcpy(state, &current_ekf_state, sizeof(state));
+}
+
+void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
+{
+    memcpy(last_error, &last_ekf_error, sizeof(last_error));
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h
new file mode 100644
index 000000000..a19ff1995
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_21states.h
@@ -0,0 +1,247 @@
+#pragma once
+
+#include "estimator_utilities.h"
+
+class AttPosEKF {
+
+public:
+
+    AttPosEKF();
+    ~AttPosEKF();
+
+    /* ##############################################
+     *
+     *   M A I N    F I L T E R    P A R A M E T E R S
+     *
+     * ########################################### */
+
+    /*
+     * parameters are defined here and initialised in
+     * the InitialiseParameters() (which is just 20 lines down)
+     */
+
+    float covTimeStepMax; // maximum time allowed between covariance predictions
+    float covDelAngMax; // maximum delta angle between covariance predictions
+    float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+
+    float yawVarScale;
+    float windVelSigma;
+    float dAngBiasSigma;
+    float dVelBiasSigma;
+    float magEarthSigma;
+    float magBodySigma;
+    float gndHgtSigma;
+
+    float vneSigma;
+    float vdSigma;
+    float posNeSigma;
+    float posDSigma;
+    float magMeasurementSigma;
+    float airspeedMeasurementSigma;
+
+    float gyroProcessNoise;
+    float accelProcessNoise;
+
+    float EAS2TAS; // ratio f true to equivalent airspeed
+
+    void InitialiseParameters()
+    {
+        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+        EAS2TAS = 1.0f;
+
+        yawVarScale = 1.0f;
+        windVelSigma = 0.1f;
+        dAngBiasSigma = 5.0e-7f;
+        dVelBiasSigma = 1e-4f;
+        magEarthSigma = 3.0e-4f;
+        magBodySigma  = 3.0e-4f;
+        gndHgtSigma  = 0.02f; // assume 2% terrain gradient 1-sigma
+
+        vneSigma = 0.2f;
+        vdSigma = 0.3f;
+        posNeSigma = 2.0f;
+        posDSigma = 2.0f;
+
+        magMeasurementSigma = 0.05;
+        airspeedMeasurementSigma = 1.4f;
+        gyroProcessNoise = 1.4544411e-2f;
+        accelProcessNoise = 0.5f;
+    }
+
+    // Global variables
+    float KH[n_states][n_states]; //  intermediate result used for covariance updates
+    float KHP[n_states][n_states]; // intermediate result used for covariance updates
+    float P[n_states][n_states]; // covariance matrix
+    float Kfusion[n_states]; // Kalman gains
+    float states[n_states]; // state matrix
+    float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
+    uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
+
+    float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
+    float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
+    float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
+
+    Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
+    Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
+    float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
+    Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
+    Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+    Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
+    Vector3f dVelIMU;
+    Vector3f dAngIMU;
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
+    float innovVelPos[6]; // innovation output
+    float varInnovVelPos[6]; // innovation variance output
+
+    float velNED[3]; // North, East, Down velocity obs (m/s)
+    float posNE[2]; // North, East position obs (m)
+    float hgtMea; //  measured height (m)
+    float posNED[3]; // North, East Down position (m)
+
+    float innovMag[3]; // innovation output
+    float varInnovMag[3]; // innovation variance output
+    Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
+    float innovVtas; // innovation output
+    float varInnovVtas; // innovation variance output
+    float VtasMeas; // true airspeed measurement (m/s)
+    float magDeclination;
+    float latRef; // WGS-84 latitude of reference point (rad)
+    float lonRef; // WGS-84 longitude of reference point (rad)
+    float hgtRef; // WGS-84 height of reference point (m)
+    Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
+    uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+
+    // GPS input data variables
+    float gpsCourse;
+    float gpsVelD;
+    float gpsLat;
+    float gpsLon;
+    float gpsHgt;
+    uint8_t GPSstatus;
+
+    // Baro input
+    float baroHgt;
+
+    bool statesInitialised;
+
+    bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
+    bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
+    bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
+    bool fuseMagData; // boolean true when magnetometer data is to be fused
+    bool fuseVtasData; // boolean true when airspeed data is to be fused
+
+    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
+    bool staticMode;    ///< boolean true if no position feedback is fused
+    bool useAirspeed;    ///< boolean true if airspeed data is being used
+    bool useCompass;    ///< boolean true if magnetometer data is being used
+
+    struct ekf_status_report current_ekf_state;
+    struct ekf_status_report last_ekf_error;
+
+    bool numericalProtection;
+
+    unsigned storeIndex;
+
+
+void  UpdateStrapdownEquationsNED();
+
+void CovariancePrediction(float dt);
+
+void FuseVelposNED();
+
+void FuseMagnetometer();
+
+void FuseAirspeed();
+
+void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void quatNorm(float (&quatOut)[4], const float quatIn[4]);
+
+// store staes along with system time stamp in msces
+void StoreStates(uint64_t timestamp_ms);
+
+/**
+ * Recall the state vector.
+ *
+ * Recalls the vector stored at closest time to the one specified by msec
+ *
+ * @return zero on success, integer indicating the number of invalid states on failure.
+ *         Does only copy valid states, if the statesForFusion vector was initialized
+ *         correctly by the caller, the result can be safely used, but is a mixture
+ *         time-wise where valid states were updated and invalid remained at the old
+ *         value.
+ */
+int RecallStates(float statesForFusion[n_states], uint64_t msec);
+
+void ResetStoredStates();
+
+void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
+
+void calcEarthRateNED(Vector3f &omega, float latitude);
+
+static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+
+static void quat2eul(float (&eul)[3], const float (&quat)[4]);
+
+static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+
+static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+
+static float sq(float valIn);
+
+void OnGroundCheck();
+
+void CovarianceInit();
+
+void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
+
+float ConstrainFloat(float val, float min, float max);
+
+void ConstrainVariances();
+
+void ConstrainStates();
+
+void ForceSymmetry();
+
+int CheckAndBound();
+
+void ResetPosition();
+
+void ResetVelocity();
+
+void ZeroVariables();
+
+void GetFilterState(struct ekf_status_report *state);
+
+void GetLastErrorState(struct ekf_status_report *last_error);
+
+bool StatesNaN(struct ekf_status_report *err_report);
+void FillErrorReport(struct ekf_status_report *err);
+
+void InitializeDynamic(float (&initvelNED)[3], float declination);
+
+protected:
+
+bool FilterHealthy();
+
+void ResetHeight(void);
+
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
+
+};
+
+uint32_t millis();
+
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
new file mode 100644
index 000000000..768e0be35
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -0,0 +1,3046 @@
+#include "estimator_23states.h"
+#include <string.h>
+#include <stdio.h>
+#include <stdarg.h>
+
+#define EKF_COVARIANCE_DIVERGED 1.0e8f
+
+AttPosEKF::AttPosEKF() :
+    covTimeStepMax(0.0f),
+    covDelAngMax(0.0f),
+    rngFinderPitch(0.0f),
+    a1(0.0f),
+    a2(0.0f),
+    a3(0.0f),
+    yawVarScale(0.0f),
+    windVelSigma(0.0f),
+    dAngBiasSigma(0.0f),
+    dVelBiasSigma(0.0f),
+    magEarthSigma(0.0f),
+    magBodySigma(0.0f),
+    gndHgtSigma(0.0f),
+    vneSigma(0.0f),
+    vdSigma(0.0f),
+    posNeSigma(0.0f),
+    posDSigma(0.0f),
+    magMeasurementSigma(0.0f),
+    airspeedMeasurementSigma(0.0f),
+    gyroProcessNoise(0.0f),
+    accelProcessNoise(0.0f),
+    EAS2TAS(1.0f),
+    magstate{},
+    resetMagState{},
+    KH{},
+    KHP{},
+    P{},
+    Kfusion{},
+    states{},
+    resetStates{},
+    storedStates{},
+    statetimeStamp{},
+    statesAtVelTime{},
+    statesAtPosTime{},
+    statesAtHgtTime{},
+    statesAtMagMeasTime{},
+    statesAtVtasMeasTime{},
+    statesAtRngTime{},
+    statesAtOptFlowTime{},
+    correctedDelAng(),
+    correctedDelVel(),
+    summedDelAng(),
+    summedDelVel(),
+    accNavMag(),
+    earthRateNED(),
+    angRate(),
+    lastGyroOffset(),
+    delAngTotal(),
+    Tbn(),
+    Tnb(),
+    accel(),
+    dVelIMU(),
+    dAngIMU(),
+    dtIMU(0),
+    fusionModeGPS(0),
+    innovVelPos{},
+    varInnovVelPos{},
+    velNED{},
+    accelGPSNED{},
+    posNE{},
+    hgtMea(0.0f),
+    baroHgtOffset(0.0f),
+    rngMea(0.0f),
+    innovMag{},
+    varInnovMag{},
+    magData{},
+    losData{},
+    innovVtas(0.0f),
+    innovRng(0.0f),
+    innovOptFlow{},
+    varInnovOptFlow{},
+    varInnovVtas(0.0f),
+    varInnovRng(0.0f),
+    VtasMeas(0.0f),
+    magDeclination(0.0f),
+    latRef(0.0f),
+    lonRef(-M_PI_F),
+    hgtRef(0.0f),
+    refSet(false),
+    magBias(),
+    covSkipCount(0),
+    gpsLat(0.0),
+    gpsLon(-M_PI),
+    gpsHgt(0.0f),
+    GPSstatus(0),
+    baroHgt(0.0f),
+    statesInitialised(false),
+    fuseVelData(false),
+    fusePosData(false),
+    fuseHgtData(false),
+    fuseMagData(false),
+    fuseVtasData(false),
+    fuseRngData(false),
+    fuseOptFlowData(false),
+
+    inhibitWindStates(true),
+    inhibitMagStates(true),
+    inhibitGndHgtState(true),
+
+    onGround(true),
+    staticMode(true),
+    useAirspeed(true),
+    useCompass(true),
+    useRangeFinder(false),
+    useOpticalFlow(false),
+
+    ekfDiverged(false),
+    lastReset(0),
+    current_ekf_state{},
+    last_ekf_error{},
+    numericalProtection(true),
+    storeIndex(0)
+{
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
+    ZeroVariables();
+    InitialiseParameters();
+}
+
+AttPosEKF::~AttPosEKF()
+{
+}
+
+void AttPosEKF::UpdateStrapdownEquationsNED()
+{
+    Vector3f delVelNav;
+    float q00;
+    float q11;
+    float q22;
+    float q33;
+    float q01;
+    float q02;
+    float q03;
+    float q12;
+    float q13;
+    float q23;
+    float rotationMag;
+    float qUpdated[4];
+    float quatMag;
+    float deltaQuat[4];
+    const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS);
+
+// Remove sensor bias errors
+    correctedDelAng.x = dAngIMU.x - states[10];
+    correctedDelAng.y = dAngIMU.y - states[11];
+    correctedDelAng.z = dAngIMU.z - states[12];
+    dVelIMU.x = dVelIMU.x;
+    dVelIMU.y = dVelIMU.y;
+    dVelIMU.z = dVelIMU.z - states[13];
+
+    delAngTotal.x += correctedDelAng.x;
+    delAngTotal.y += correctedDelAng.y;
+    delAngTotal.z += correctedDelAng.z;
+
+// Save current measurements
+    Vector3f  prevDelAng = correctedDelAng;
+
+// Apply corrections for earths rotation rate and coning errors
+// * and + operators have been overloaded
+    correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
+
+// Convert the rotation vector to its equivalent quaternion
+    rotationMag = correctedDelAng.length();
+    if (rotationMag < 1e-12f)
+    {
+        deltaQuat[0] = 1.0;
+        deltaQuat[1] = 0.0;
+        deltaQuat[2] = 0.0;
+        deltaQuat[3] = 0.0;
+    }
+    else
+    {
+        // We are using double here as we are unsure how small
+        // the angle differences are and if we get into numeric
+        // issues with float. The runtime impact is not measurable
+        // for these quantities.
+        deltaQuat[0] = cos(0.5*(double)rotationMag);
+        float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
+        deltaQuat[1] = correctedDelAng.x*rotScaler;
+        deltaQuat[2] = correctedDelAng.y*rotScaler;
+        deltaQuat[3] = correctedDelAng.z*rotScaler;
+    }
+
+// Update the quaternions by rotating from the previous attitude through
+// the delta angle rotation quaternion
+    qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
+    qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
+    qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
+    qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
+
+// Normalise the quaternions and update the quaternion states
+    quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
+    if (quatMag > 1e-16f)
+    {
+        float quatMagInv = 1.0f/quatMag;
+        states[0] = quatMagInv*qUpdated[0];
+        states[1] = quatMagInv*qUpdated[1];
+        states[2] = quatMagInv*qUpdated[2];
+        states[3] = quatMagInv*qUpdated[3];
+    }
+
+// Calculate the body to nav cosine matrix
+    q00 = sq(states[0]);
+    q11 = sq(states[1]);
+    q22 = sq(states[2]);
+    q33 = sq(states[3]);
+    q01 =  states[0]*states[1];
+    q02 =  states[0]*states[2];
+    q03 =  states[0]*states[3];
+    q12 =  states[1]*states[2];
+    q13 =  states[1]*states[3];
+    q23 =  states[2]*states[3];
+
+    Tbn.x.x = q00 + q11 - q22 - q33;
+    Tbn.y.y = q00 - q11 + q22 - q33;
+    Tbn.z.z = q00 - q11 - q22 + q33;
+    Tbn.x.y = 2*(q12 - q03);
+    Tbn.x.z = 2*(q13 + q02);
+    Tbn.y.x = 2*(q12 + q03);
+    Tbn.y.z = 2*(q23 - q01);
+    Tbn.z.x = 2*(q13 - q02);
+    Tbn.z.y = 2*(q23 + q01);
+
+    Tnb = Tbn.transpose();
+
+// transform body delta velocities to delta velocities in the nav frame
+// * and + operators have been overloaded
+    //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
+    delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
+    delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
+    delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
+
+// calculate the magnitude of the nav acceleration (required for GPS
+// variance estimation)
+    accNavMag = delVelNav.length()/dtIMU;
+
+// If calculating position save previous velocity
+    float lastVelocity[3];
+    lastVelocity[0] = states[4];
+    lastVelocity[1] = states[5];
+    lastVelocity[2] = states[6];
+
+// Sum delta velocities to get velocity
+    states[4] = states[4] + delVelNav.x;
+    states[5] = states[5] + delVelNav.y;
+    states[6] = states[6] + delVelNav.z;
+
+// If calculating postions, do a trapezoidal integration for position
+    states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
+    states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
+    states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
+
+    // Constrain states (to protect against filter divergence)
+    ConstrainStates();
+}
+
+void AttPosEKF::CovariancePrediction(float dt)
+{
+    // scalars
+    float daxCov;
+    float dayCov;
+    float dazCov;
+    float dvxCov;
+    float dvyCov;
+    float dvzCov;
+    float dvx;
+    float dvy;
+    float dvz;
+    float dax;
+    float day;
+    float daz;
+    float q0;
+    float q1;
+    float q2;
+    float q3;
+    float dax_b;
+    float day_b;
+    float daz_b;
+    float dvz_b;
+
+    // arrays
+    float processNoise[n_states];
+    float SF[15];
+    float SG[8];
+    float SQ[11];
+    float SPP[8] = {0};
+    float nextP[n_states][n_states];
+
+    // calculate covariance prediction process noise
+    for (uint8_t i= 0; i<4;  i++) processNoise[i] = 1.0e-9f;
+    for (uint8_t i= 4; i<10;  i++) processNoise[i] = 1.0e-9f;
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
+    // scale gyro bias noise when on ground to allow for faster bias estimation
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
+    processNoise[13] = dVelBiasSigma;
+    if (!inhibitWindStates) {
+        for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
+    } else {
+        for (uint8_t i=14; i<=15; i++) processNoise[i] = 0;
+    }
+    if (!inhibitMagStates) {
+        for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma;
+        for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma;
+    } else {
+        for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
+    }
+    if (!inhibitGndHgtState) {
+        processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
+    } else {
+        processNoise[22] = 0;
+    }
+
+    // square all sigmas
+    for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]);
+
+    // set variables used to calculate covariance growth
+    dvx = summedDelVel.x;
+    dvy = summedDelVel.y;
+    dvz = summedDelVel.z;
+    dax = summedDelAng.x;
+    day = summedDelAng.y;
+    daz = summedDelAng.z;
+    q0 = states[0];
+    q1 = states[1];
+    q2 = states[2];
+    q3 = states[3];
+    dax_b = states[10];
+    day_b = states[11];
+    daz_b = states[12];
+    dvz_b =  states[13];
+    gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f);
+    daxCov = sq(dt*gyroProcessNoise);
+    dayCov = sq(dt*gyroProcessNoise);
+    dazCov = sq(dt*gyroProcessNoise);
+    if (onGround) dazCov = dazCov * sq(yawVarScale);
+    accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f);
+    dvxCov = sq(dt*accelProcessNoise);
+    dvyCov = sq(dt*accelProcessNoise);
+    dvzCov = sq(dt*accelProcessNoise);
+
+    // Predicted covariance calculation
+    SF[0] = dvz - dvz_b;
+    SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2;
+    SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
+    SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
+    SF[4] = day/2 - day_b/2;
+    SF[5] = daz/2 - daz_b/2;
+    SF[6] = dax/2 - dax_b/2;
+    SF[7] = dax_b/2 - dax/2;
+    SF[8] = daz_b/2 - daz/2;
+    SF[9] = day_b/2 - day/2;
+    SF[10] = 2*q0*SF[0];
+    SF[11] = q1/2;
+    SF[12] = q2/2;
+    SF[13] = q3/2;
+    SF[14] = 2*dvy*q1;
+
+    SG[0] = q0/2;
+    SG[1] = sq(q3);
+    SG[2] = sq(q2);
+    SG[3] = sq(q1);
+    SG[4] = sq(q0);
+    SG[5] = 2*q2*q3;
+    SG[6] = 2*q1*q3;
+    SG[7] = 2*q1*q2;
+
+    SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
+    SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
+    SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
+    SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4;
+    SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4;
+    SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4;
+    SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4;
+    SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2;
+    SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4;
+    SQ[9] = sq(SG[0]);
+    SQ[10] = sq(q1);
+
+    SPP[0] = SF[10] + SF[14] - 2*dvx*q2;
+    SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
+    SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
+    SPP[3] = 2*q0*q1 - 2*q2*q3;
+    SPP[4] = 2*q0*q2 + 2*q1*q3;
+    SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
+    SPP[6] = SF[13];
+    SPP[7] = SF[12];
+
+    nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
+    nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2;
+    nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2;
+    nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2;
+    nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+    nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+    nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+    nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]);
+    nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]);
+    nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]);
+    nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6];
+    nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6];
+    nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6];
+    nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6];
+    nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6];
+    nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6];
+    nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6];
+    nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6];
+    nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6];
+    nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6];
+    nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6];
+    nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6];
+    nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6];
+    nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2);
+    nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2;
+    nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2;
+    nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2;
+    nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
+    nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
+    nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
+    nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2);
+    nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2);
+    nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2);
+    nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2;
+    nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2;
+    nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2;
+    nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2;
+    nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2;
+    nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2;
+    nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2;
+    nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2;
+    nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2;
+    nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2;
+    nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2;
+    nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2;
+    nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2;
+    nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2);
+    nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2;
+    nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2;
+    nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2;
+    nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
+    nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
+    nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
+    nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2);
+    nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2);
+    nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2);
+    nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2;
+    nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2;
+    nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2;
+    nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2;
+    nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2;
+    nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2;
+    nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2;
+    nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2;
+    nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2;
+    nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2;
+    nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2;
+    nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2;
+    nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2;
+    nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2);
+    nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2;
+    nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2;
+    nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2;
+    nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
+    nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
+    nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
+    nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2);
+    nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2);
+    nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2);
+    nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2;
+    nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2;
+    nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2;
+    nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2;
+    nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2;
+    nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2;
+    nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2;
+    nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2;
+    nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2;
+    nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2;
+    nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2;
+    nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2;
+    nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2;
+    nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]);
+    nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2;
+    nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2;
+    nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2;
+    nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]);
+    nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]);
+    nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]);
+    nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]);
+    nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]);
+    nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]);
+    nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4];
+    nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4];
+    nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4];
+    nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4];
+    nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4];
+    nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4];
+    nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4];
+    nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4];
+    nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4];
+    nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4];
+    nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4];
+    nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4];
+    nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4];
+    nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]);
+    nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2;
+    nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2;
+    nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2;
+    nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]);
+    nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]);
+    nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]);
+    nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]);
+    nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]);
+    nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]);
+    nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3];
+    nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3];
+    nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3];
+    nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3];
+    nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3];
+    nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3];
+    nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3];
+    nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3];
+    nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3];
+    nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3];
+    nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3];
+    nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3];
+    nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3];
+    nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
+    nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
+    nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
+    nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
+    nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
+    nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
+    nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]);
+    nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]);
+    nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]);
+    nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]);
+    nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5];
+    nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5];
+    nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5];
+    nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5];
+    nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5];
+    nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5];
+    nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5];
+    nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5];
+    nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5];
+    nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5];
+    nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5];
+    nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5];
+    nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5];
+    nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt);
+    nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2;
+    nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
+    nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2;
+    nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt) - SPP[4]*(P[7][13] + P[4][13]*dt);
+    nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt);
+    nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt) - SPP[5]*(P[7][13] + P[4][13]*dt);
+    nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
+    nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
+    nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
+    nextP[7][10] = P[7][10] + P[4][10]*dt;
+    nextP[7][11] = P[7][11] + P[4][11]*dt;
+    nextP[7][12] = P[7][12] + P[4][12]*dt;
+    nextP[7][13] = P[7][13] + P[4][13]*dt;
+    nextP[7][14] = P[7][14] + P[4][14]*dt;
+    nextP[7][15] = P[7][15] + P[4][15]*dt;
+    nextP[7][16] = P[7][16] + P[4][16]*dt;
+    nextP[7][17] = P[7][17] + P[4][17]*dt;
+    nextP[7][18] = P[7][18] + P[4][18]*dt;
+    nextP[7][19] = P[7][19] + P[4][19]*dt;
+    nextP[7][20] = P[7][20] + P[4][20]*dt;
+    nextP[7][21] = P[7][21] + P[4][21]*dt;
+    nextP[7][22] = P[7][22] + P[4][22]*dt;
+    nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt);
+    nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2;
+    nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
+    nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2;
+    nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt) - SPP[4]*(P[8][13] + P[5][13]*dt);
+    nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt);
+    nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt) - SPP[5]*(P[8][13] + P[5][13]*dt);
+    nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt);
+    nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
+    nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
+    nextP[8][10] = P[8][10] + P[5][10]*dt;
+    nextP[8][11] = P[8][11] + P[5][11]*dt;
+    nextP[8][12] = P[8][12] + P[5][12]*dt;
+    nextP[8][13] = P[8][13] + P[5][13]*dt;
+    nextP[8][14] = P[8][14] + P[5][14]*dt;
+    nextP[8][15] = P[8][15] + P[5][15]*dt;
+    nextP[8][16] = P[8][16] + P[5][16]*dt;
+    nextP[8][17] = P[8][17] + P[5][17]*dt;
+    nextP[8][18] = P[8][18] + P[5][18]*dt;
+    nextP[8][19] = P[8][19] + P[5][19]*dt;
+    nextP[8][20] = P[8][20] + P[5][20]*dt;
+    nextP[8][21] = P[8][21] + P[5][21]*dt;
+    nextP[8][22] = P[8][22] + P[5][22]*dt;
+    nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt);
+    nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2;
+    nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
+    nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2;
+    nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt) - SPP[4]*(P[9][13] + P[6][13]*dt);
+    nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt);
+    nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt) - SPP[5]*(P[9][13] + P[6][13]*dt);
+    nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt);
+    nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt);
+    nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
+    nextP[9][10] = P[9][10] + P[6][10]*dt;
+    nextP[9][11] = P[9][11] + P[6][11]*dt;
+    nextP[9][12] = P[9][12] + P[6][12]*dt;
+    nextP[9][13] = P[9][13] + P[6][13]*dt;
+    nextP[9][14] = P[9][14] + P[6][14]*dt;
+    nextP[9][15] = P[9][15] + P[6][15]*dt;
+    nextP[9][16] = P[9][16] + P[6][16]*dt;
+    nextP[9][17] = P[9][17] + P[6][17]*dt;
+    nextP[9][18] = P[9][18] + P[6][18]*dt;
+    nextP[9][19] = P[9][19] + P[6][19]*dt;
+    nextP[9][20] = P[9][20] + P[6][20]*dt;
+    nextP[9][21] = P[9][21] + P[6][21]*dt;
+    nextP[9][22] = P[9][22] + P[6][22]*dt;
+    nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6];
+    nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2;
+    nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2;
+    nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2;
+    nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4];
+    nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3];
+    nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5];
+    nextP[10][7] = P[10][7] + P[10][4]*dt;
+    nextP[10][8] = P[10][8] + P[10][5]*dt;
+    nextP[10][9] = P[10][9] + P[10][6]*dt;
+    nextP[10][10] = P[10][10];
+    nextP[10][11] = P[10][11];
+    nextP[10][12] = P[10][12];
+    nextP[10][13] = P[10][13];
+    nextP[10][14] = P[10][14];
+    nextP[10][15] = P[10][15];
+    nextP[10][16] = P[10][16];
+    nextP[10][17] = P[10][17];
+    nextP[10][18] = P[10][18];
+    nextP[10][19] = P[10][19];
+    nextP[10][20] = P[10][20];
+    nextP[10][21] = P[10][21];
+    nextP[10][22] = P[10][22];
+    nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6];
+    nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2;
+    nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2;
+    nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2;
+    nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4];
+    nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3];
+    nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5];
+    nextP[11][7] = P[11][7] + P[11][4]*dt;
+    nextP[11][8] = P[11][8] + P[11][5]*dt;
+    nextP[11][9] = P[11][9] + P[11][6]*dt;
+    nextP[11][10] = P[11][10];
+    nextP[11][11] = P[11][11];
+    nextP[11][12] = P[11][12];
+    nextP[11][13] = P[11][13];
+    nextP[11][14] = P[11][14];
+    nextP[11][15] = P[11][15];
+    nextP[11][16] = P[11][16];
+    nextP[11][17] = P[11][17];
+    nextP[11][18] = P[11][18];
+    nextP[11][19] = P[11][19];
+    nextP[11][20] = P[11][20];
+    nextP[11][21] = P[11][21];
+    nextP[11][22] = P[11][22];
+    nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6];
+    nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2;
+    nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2;
+    nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2;
+    nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4];
+    nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3];
+    nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5];
+    nextP[12][7] = P[12][7] + P[12][4]*dt;
+    nextP[12][8] = P[12][8] + P[12][5]*dt;
+    nextP[12][9] = P[12][9] + P[12][6]*dt;
+    nextP[12][10] = P[12][10];
+    nextP[12][11] = P[12][11];
+    nextP[12][12] = P[12][12];
+    nextP[12][13] = P[12][13];
+    nextP[12][14] = P[12][14];
+    nextP[12][15] = P[12][15];
+    nextP[12][16] = P[12][16];
+    nextP[12][17] = P[12][17];
+    nextP[12][18] = P[12][18];
+    nextP[12][19] = P[12][19];
+    nextP[12][20] = P[12][20];
+    nextP[12][21] = P[12][21];
+    nextP[12][22] = P[12][22];
+    nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6];
+    nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2;
+    nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2;
+    nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2;
+    nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4];
+    nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3];
+    nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5];
+    nextP[13][7] = P[13][7] + P[13][4]*dt;
+    nextP[13][8] = P[13][8] + P[13][5]*dt;
+    nextP[13][9] = P[13][9] + P[13][6]*dt;
+    nextP[13][10] = P[13][10];
+    nextP[13][11] = P[13][11];
+    nextP[13][12] = P[13][12];
+    nextP[13][13] = P[13][13];
+    nextP[13][14] = P[13][14];
+    nextP[13][15] = P[13][15];
+    nextP[13][16] = P[13][16];
+    nextP[13][17] = P[13][17];
+    nextP[13][18] = P[13][18];
+    nextP[13][19] = P[13][19];
+    nextP[13][20] = P[13][20];
+    nextP[13][21] = P[13][21];
+    nextP[13][22] = P[13][22];
+    nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6];
+    nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2;
+    nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2;
+    nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2;
+    nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4];
+    nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3];
+    nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5];
+    nextP[14][7] = P[14][7] + P[14][4]*dt;
+    nextP[14][8] = P[14][8] + P[14][5]*dt;
+    nextP[14][9] = P[14][9] + P[14][6]*dt;
+    nextP[14][10] = P[14][10];
+    nextP[14][11] = P[14][11];
+    nextP[14][12] = P[14][12];
+    nextP[14][13] = P[14][13];
+    nextP[14][14] = P[14][14];
+    nextP[14][15] = P[14][15];
+    nextP[14][16] = P[14][16];
+    nextP[14][17] = P[14][17];
+    nextP[14][18] = P[14][18];
+    nextP[14][19] = P[14][19];
+    nextP[14][20] = P[14][20];
+    nextP[14][21] = P[14][21];
+    nextP[14][22] = P[14][22];
+    nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6];
+    nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2;
+    nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2;
+    nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2;
+    nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4];
+    nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3];
+    nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5];
+    nextP[15][7] = P[15][7] + P[15][4]*dt;
+    nextP[15][8] = P[15][8] + P[15][5]*dt;
+    nextP[15][9] = P[15][9] + P[15][6]*dt;
+    nextP[15][10] = P[15][10];
+    nextP[15][11] = P[15][11];
+    nextP[15][12] = P[15][12];
+    nextP[15][13] = P[15][13];
+    nextP[15][14] = P[15][14];
+    nextP[15][15] = P[15][15];
+    nextP[15][16] = P[15][16];
+    nextP[15][17] = P[15][17];
+    nextP[15][18] = P[15][18];
+    nextP[15][19] = P[15][19];
+    nextP[15][20] = P[15][20];
+    nextP[15][21] = P[15][21];
+    nextP[15][22] = P[15][22];
+    nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6];
+    nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2;
+    nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2;
+    nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2;
+    nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4];
+    nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3];
+    nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5];
+    nextP[16][7] = P[16][7] + P[16][4]*dt;
+    nextP[16][8] = P[16][8] + P[16][5]*dt;
+    nextP[16][9] = P[16][9] + P[16][6]*dt;
+    nextP[16][10] = P[16][10];
+    nextP[16][11] = P[16][11];
+    nextP[16][12] = P[16][12];
+    nextP[16][13] = P[16][13];
+    nextP[16][14] = P[16][14];
+    nextP[16][15] = P[16][15];
+    nextP[16][16] = P[16][16];
+    nextP[16][17] = P[16][17];
+    nextP[16][18] = P[16][18];
+    nextP[16][19] = P[16][19];
+    nextP[16][20] = P[16][20];
+    nextP[16][21] = P[16][21];
+    nextP[16][22] = P[16][22];
+    nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6];
+    nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2;
+    nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2;
+    nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2;
+    nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4];
+    nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3];
+    nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5];
+    nextP[17][7] = P[17][7] + P[17][4]*dt;
+    nextP[17][8] = P[17][8] + P[17][5]*dt;
+    nextP[17][9] = P[17][9] + P[17][6]*dt;
+    nextP[17][10] = P[17][10];
+    nextP[17][11] = P[17][11];
+    nextP[17][12] = P[17][12];
+    nextP[17][13] = P[17][13];
+    nextP[17][14] = P[17][14];
+    nextP[17][15] = P[17][15];
+    nextP[17][16] = P[17][16];
+    nextP[17][17] = P[17][17];
+    nextP[17][18] = P[17][18];
+    nextP[17][19] = P[17][19];
+    nextP[17][20] = P[17][20];
+    nextP[17][21] = P[17][21];
+    nextP[17][22] = P[17][22];
+    nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6];
+    nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2;
+    nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2;
+    nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2;
+    nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4];
+    nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3];
+    nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5];
+    nextP[18][7] = P[18][7] + P[18][4]*dt;
+    nextP[18][8] = P[18][8] + P[18][5]*dt;
+    nextP[18][9] = P[18][9] + P[18][6]*dt;
+    nextP[18][10] = P[18][10];
+    nextP[18][11] = P[18][11];
+    nextP[18][12] = P[18][12];
+    nextP[18][13] = P[18][13];
+    nextP[18][14] = P[18][14];
+    nextP[18][15] = P[18][15];
+    nextP[18][16] = P[18][16];
+    nextP[18][17] = P[18][17];
+    nextP[18][18] = P[18][18];
+    nextP[18][19] = P[18][19];
+    nextP[18][20] = P[18][20];
+    nextP[18][21] = P[18][21];
+    nextP[18][22] = P[18][22];
+    nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6];
+    nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2;
+    nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2;
+    nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2;
+    nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4];
+    nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3];
+    nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5];
+    nextP[19][7] = P[19][7] + P[19][4]*dt;
+    nextP[19][8] = P[19][8] + P[19][5]*dt;
+    nextP[19][9] = P[19][9] + P[19][6]*dt;
+    nextP[19][10] = P[19][10];
+    nextP[19][11] = P[19][11];
+    nextP[19][12] = P[19][12];
+    nextP[19][13] = P[19][13];
+    nextP[19][14] = P[19][14];
+    nextP[19][15] = P[19][15];
+    nextP[19][16] = P[19][16];
+    nextP[19][17] = P[19][17];
+    nextP[19][18] = P[19][18];
+    nextP[19][19] = P[19][19];
+    nextP[19][20] = P[19][20];
+    nextP[19][21] = P[19][21];
+    nextP[19][22] = P[19][22];
+    nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6];
+    nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2;
+    nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2;
+    nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2;
+    nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4];
+    nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3];
+    nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5];
+    nextP[20][7] = P[20][7] + P[20][4]*dt;
+    nextP[20][8] = P[20][8] + P[20][5]*dt;
+    nextP[20][9] = P[20][9] + P[20][6]*dt;
+    nextP[20][10] = P[20][10];
+    nextP[20][11] = P[20][11];
+    nextP[20][12] = P[20][12];
+    nextP[20][13] = P[20][13];
+    nextP[20][14] = P[20][14];
+    nextP[20][15] = P[20][15];
+    nextP[20][16] = P[20][16];
+    nextP[20][17] = P[20][17];
+    nextP[20][18] = P[20][18];
+    nextP[20][19] = P[20][19];
+    nextP[20][20] = P[20][20];
+    nextP[20][21] = P[20][21];
+    nextP[20][22] = P[20][22];
+    nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6];
+    nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2;
+    nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2;
+    nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2;
+    nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4];
+    nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3];
+    nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5];
+    nextP[21][7] = P[21][7] + P[21][4]*dt;
+    nextP[21][8] = P[21][8] + P[21][5]*dt;
+    nextP[21][9] = P[21][9] + P[21][6]*dt;
+    nextP[21][10] = P[21][10];
+    nextP[21][11] = P[21][11];
+    nextP[21][12] = P[21][12];
+    nextP[21][13] = P[21][13];
+    nextP[21][14] = P[21][14];
+    nextP[21][15] = P[21][15];
+    nextP[21][16] = P[21][16];
+    nextP[21][17] = P[21][17];
+    nextP[21][18] = P[21][18];
+    nextP[21][19] = P[21][19];
+    nextP[21][20] = P[21][20];
+    nextP[21][21] = P[21][21];
+    nextP[21][22] = P[21][22];
+    nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6];
+    nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2;
+    nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2;
+    nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2;
+    nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4];
+    nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3];
+    nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5];
+    nextP[22][7] = P[22][7] + P[22][4]*dt;
+    nextP[22][8] = P[22][8] + P[22][5]*dt;
+    nextP[22][9] = P[22][9] + P[22][6]*dt;
+    nextP[22][10] = P[22][10];
+    nextP[22][11] = P[22][11];
+    nextP[22][12] = P[22][12];
+    nextP[22][13] = P[22][13];
+    nextP[22][14] = P[22][14];
+    nextP[22][15] = P[22][15];
+    nextP[22][16] = P[22][16];
+    nextP[22][17] = P[22][17];
+    nextP[22][18] = P[22][18];
+    nextP[22][19] = P[22][19];
+    nextP[22][20] = P[22][20];
+    nextP[22][21] = P[22][21];
+    nextP[22][22] = P[22][22];
+
+    for (unsigned i = 0; i < n_states; i++)
+    {
+        nextP[i][i] = nextP[i][i] + processNoise[i];
+    }
+
+    // If the total position variance exceds 1E6 (1000m), then stop covariance
+    // growth by setting the predicted to the previous values
+    // This prevent an ill conditioned matrix from occurring for long periods
+    // without GPS
+    if ((P[7][7] + P[8][8]) > 1E6f)
+    {
+        for (uint8_t i=7; i<=8; i++)
+        {
+            for (unsigned j = 0; j < n_states; j++)
+            {
+                nextP[i][j] = P[i][j];
+                nextP[j][i] = P[j][i];
+            }
+        }
+    }
+
+    // Copy covariance
+    for (unsigned i = 0; i < n_states; i++) {
+        P[i][i] = nextP[i][i];
+    }
+
+    // force symmetry for observable states
+    for (unsigned i = 1; i < n_states; i++)
+    {
+        for (uint8_t j = 0; j < i; j++)
+        {
+            P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+            P[j][i] = P[i][j];
+        }
+    }
+
+        ConstrainVariances();
+}
+
+void AttPosEKF::FuseVelposNED()
+{
+
+// declare variables used by fault isolation logic
+    uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure
+    uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available
+    uint32_t hgtRetryTime = 500; // height measurement retry time
+    uint32_t horizRetryTime;
+
+// declare variables used to check measurement errors
+    float velInnov[3] = {0.0f,0.0f,0.0f};
+    float posInnov[2] = {0.0f,0.0f};
+    float hgtInnov = 0.0f;
+
+// declare variables used to control access to arrays
+    bool fuseData[6] = {false,false,false,false,false,false};
+    uint8_t stateIndex;
+    uint8_t obsIndex;
+    uint8_t indexLimit = 22;
+
+// declare variables used by state and covariance update calculations
+    float velErr;
+    float posErr;
+    float R_OBS[6];
+    float observation[6];
+    float SK;
+    float quatMag;
+
+// Perform sequential fusion of GPS measurements. This assumes that the
+// errors in the different velocity and position components are
+// uncorrelated which is not true, however in the absence of covariance
+// data from the GPS receiver it is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+    if (fuseVelData || fusePosData || fuseHgtData)
+    {
+        // set the GPS data timeout depending on whether airspeed data is present
+        if (useAirspeed) horizRetryTime = gpsRetryTime;
+        else horizRetryTime = gpsRetryTimeNoTAS;
+
+        // Form the observation vector
+        for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
+        for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
+        observation[5] = -(hgtMea);
+
+        // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
+        velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
+        posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
+        R_OBS[0] = sq(vneSigma) + sq(velErr);
+        R_OBS[1] = R_OBS[0];
+        R_OBS[2] = sq(vdSigma) + sq(velErr);
+        R_OBS[3] = sq(posNeSigma) + sq(posErr);
+        R_OBS[4] = R_OBS[3];
+        R_OBS[5] = sq(posDSigma) + sq(posErr);
+
+        // calculate innovations and check GPS data validity using an innovation consistency check
+        if (fuseVelData)
+        {
+            // test velocity measurements
+            uint8_t imax = 2;
+            if (fusionModeGPS == 1) imax = 1;
+            for (uint8_t i = 0; i<=imax; i++)
+            {
+                velInnov[i] = statesAtVelTime[i+4] - velNED[i];
+                stateIndex = 4 + i;
+                varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
+            }
+            // apply a 5-sigma threshold
+            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
+            current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
+            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
+            {
+                current_ekf_state.velHealth = true;
+                current_ekf_state.velFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.velHealth = false;
+            }
+        }
+        if (fusePosData)
+        {
+            // test horizontal position measurements
+            posInnov[0] = statesAtPosTime[7] - posNE[0];
+            posInnov[1] = statesAtPosTime[8] - posNE[1];
+            varInnovVelPos[3] = P[7][7] + R_OBS[3];
+            varInnovVelPos[4] = P[8][8] + R_OBS[4];
+            // apply a 10-sigma threshold
+            current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
+            current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
+            if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
+            {
+                current_ekf_state.posHealth = true;
+                current_ekf_state.posFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.posHealth = false;
+            }
+        }
+        // test height measurements
+        if (fuseHgtData)
+        {
+            hgtInnov = statesAtHgtTime[9] + hgtMea;
+            varInnovVelPos[5] = P[9][9] + R_OBS[5];
+            // apply a 10-sigma threshold
+            current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
+            current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
+            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
+            {
+                current_ekf_state.hgtHealth = true;
+                current_ekf_state.hgtFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.hgtHealth = false;
+            }
+        }
+        // Set range for sequential fusion of velocity and position measurements depending
+        // on which data is available and its health
+        if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth)
+        {
+            fuseData[0] = true;
+            fuseData[1] = true;
+            fuseData[2] = true;
+        }
+        if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth)
+        {
+            fuseData[0] = true;
+            fuseData[1] = true;
+        }
+        if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth)
+        {
+            fuseData[3] = true;
+            fuseData[4] = true;
+        }
+        if (fuseHgtData && current_ekf_state.hgtHealth)
+        {
+            fuseData[5] = true;
+        }
+        // Fuse measurements sequentially
+        for (obsIndex=0; obsIndex<=5; obsIndex++)
+        {
+            if (fuseData[obsIndex])
+            {
+                stateIndex = 4 + obsIndex;
+                // Calculate the measurement innovation, using states from a
+                // different time coordinate if fusing height data
+                if (obsIndex <= 2)
+                {
+                    innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
+                }
+                else if (obsIndex == 3 || obsIndex == 4)
+                {
+                    innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
+                }
+                else if (obsIndex == 5)
+                {
+                    innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
+                }
+                // Calculate the Kalman Gain
+                // Calculate innovation variances - also used for data logging
+                varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
+                SK = 1.0/(double)varInnovVelPos[obsIndex];
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    Kfusion[i] = P[i][stateIndex]*SK;
+                }
+
+                // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased)
+                if (obsIndex != 5) {
+                    Kfusion[13] = 0;
+                }
+                // Don't update wind states if inhibited
+                if (inhibitWindStates) {
+                    Kfusion[14] = 0;
+                    Kfusion[15] = 0;
+                }
+                // Don't update magnetic field states if inhibited
+                if (inhibitMagStates) {
+                    for (uint8_t i = 16; i<=21; i++)
+                    {
+                        Kfusion[i] = 0;
+                    }
+                }
+                // Don't update terrain state if inhibited
+                if (inhibitGndHgtState) {
+                    Kfusion[22] = 0;
+               }
+
+                // Calculate state corrections and re-normalise the quaternions
+                for (uint8_t i = 0; i<=indexLimit; i++)
+                {
+                    states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
+                }
+                quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+                if (quatMag > 1e-12f) // divide by  0 protection
+                {
+                    for (uint8_t i = 0; i<=3; i++)
+                    {
+                        states[i] = states[i] / quatMag;
+                    }
+                }
+                // Update the covariance - take advantage of direct observation of a
+                // single state at index = stateIndex to reduce computations
+                // Optimised implementation of standard equation P = (I - K*H)*P;
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    for (uint8_t j= 0; j<=indexLimit; j++)
+                    {
+                        KHP[i][j] = Kfusion[i] * P[stateIndex][j];
+                    }
+                }
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    for (uint8_t j= 0; j<=indexLimit; j++)
+                    {
+                        P[i][j] = P[i][j] - KHP[i][j];
+                    }
+                }
+            }
+        }
+    }
+
+    ForceSymmetry();
+    ConstrainVariances();
+
+}
+
+void AttPosEKF::FuseMagnetometer()
+{
+
+    float &q0 = magstate.q0;
+    float &q1 = magstate.q1;
+    float &q2 = magstate.q2;
+    float &q3 = magstate.q3;
+    float &magN = magstate.magN;
+    float &magE = magstate.magE;
+    float &magD = magstate.magD;
+    float &magXbias = magstate.magXbias;
+    float &magYbias = magstate.magYbias;
+    float &magZbias = magstate.magZbias;
+    unsigned &obsIndex = magstate.obsIndex;
+    Mat3f &DCM = magstate.DCM;
+    float *MagPred = &magstate.MagPred[0];
+    float &R_MAG = magstate.R_MAG;
+    float *SH_MAG = &magstate.SH_MAG[0];
+
+    float SK_MX[6];
+    float SK_MY[5];
+    float SK_MZ[6];
+    float H_MAG[n_states];
+    for (uint8_t i = 0; i < n_states; i++) {
+        H_MAG[i] = 0.0f;
+    }
+
+// Perform sequential fusion of Magnetometer measurements.
+// This assumes that the errors in the different components are
+// uncorrelated which is not true, however in the absence of covariance
+// data fit is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+    if (useCompass && fuseMagData && (obsIndex < 3))
+    {
+        // Calculate observation jacobians and Kalman gains
+        if (obsIndex == 0)
+        {
+            // Copy required states to local variable names
+            q0       = statesAtMagMeasTime[0];
+            q1       = statesAtMagMeasTime[1];
+            q2       = statesAtMagMeasTime[2];
+            q3       = statesAtMagMeasTime[3];
+            magN     = statesAtMagMeasTime[16];
+            magE     = statesAtMagMeasTime[17];
+            magD     = statesAtMagMeasTime[18];
+            magXbias = statesAtMagMeasTime[19];
+            magYbias = statesAtMagMeasTime[20];
+            magZbias = statesAtMagMeasTime[21];
+
+            // rotate predicted earth components into body axes and calculate
+            // predicted measurments
+            DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+            DCM.x.y = 2*(q1*q2 + q0*q3);
+            DCM.x.z = 2*(q1*q3-q0*q2);
+            DCM.y.x = 2*(q1*q2 - q0*q3);
+            DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3;
+            DCM.y.z = 2*(q2*q3 + q0*q1);
+            DCM.z.x = 2*(q1*q3 + q0*q2);
+            DCM.z.y = 2*(q2*q3 - q0*q1);
+            DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+            MagPred[0] = DCM.x.x*magN + DCM.x.y*magE  + DCM.x.z*magD + magXbias;
+            MagPred[1] = DCM.y.x*magN + DCM.y.y*magE  + DCM.y.z*magD + magYbias;
+            MagPred[2] = DCM.z.x*magN + DCM.z.y*magE  + DCM.z.z*magD + magZbias;
+
+            // scale magnetometer observation error with total angular rate
+            R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU);
+
+            // Calculate observation jacobians
+            SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
+            SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+            SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+            SH_MAG[3] = sq(q3);
+            SH_MAG[4] = sq(q2);
+            SH_MAG[5] = sq(q1);
+            SH_MAG[6] = sq(q0);
+            SH_MAG[7] = 2*magN*q0;
+            SH_MAG[8] = 2*magE*q3;
+
+            for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            H_MAG[1] = SH_MAG[0];
+            H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
+            H_MAG[3] = SH_MAG[2];
+            H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
+            H_MAG[17] = 2*q0*q3 + 2*q1*q2;
+            H_MAG[18] = 2*q1*q3 - 2*q0*q2;
+            H_MAG[19] = 1.0f;
+
+            // Calculate Kalman gain
+            float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MX[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[19][19] += 0.1f*R_MAG;
+                obsIndex = 1;
+                return;
+            }
+            SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
+            SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+            SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MX[4] = 2*q0*q2 - 2*q1*q3;
+            SK_MX[5] = 2*q0*q3 + 2*q1*q2;
+            Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]);
+            Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]);
+            Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]);
+            Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]);
+            Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]);
+            Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]);
+            Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]);
+            Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]);
+            Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]);
+            Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]);
+            Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]);
+            Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]);
+            Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]);
+            // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
+            Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]);
+            // Estimation of selected states is inhibited by setting their Kalman gains to zero
+            if (!inhibitWindStates) {
+                Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
+                Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
+            } else {
+                Kfusion[14] = 0;
+                Kfusion[15] = 0;
+            }
+            if (!inhibitMagStates) {
+                Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
+                Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
+                Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
+                Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
+                Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
+                Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
+            } else {
+                for (uint8_t i=16; i <= 21; i++) {
+                    Kfusion[i] = 0;
+                }
+            }
+            if (!inhibitGndHgtState) {
+                Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
+            } else {
+                Kfusion[22] = 0;
+            }
+            varInnovMag[0] = 1.0f/SK_MX[0];
+            innovMag[0] = MagPred[0] - magData.x;
+        }
+        else if (obsIndex == 1) // we are now fusing the Y measurement
+        {
+            // Calculate observation jacobians
+            for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[2];
+            H_MAG[1] = SH_MAG[1];
+            H_MAG[2] = SH_MAG[0];
+            H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
+            H_MAG[16] = 2*q1*q2 - 2*q0*q3;
+            H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[18] = 2*q0*q1 + 2*q2*q3;
+            H_MAG[20] = 1;
+
+            // Calculate Kalman gain
+            float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MY[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[20][20] += 0.1f*R_MAG;
+                obsIndex = 2;
+                return;
+            }
+            SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
+            SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MY[3] = 2*q0*q3 - 2*q1*q2;
+            SK_MY[4] = 2*q0*q1 + 2*q2*q3;
+            Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
+            Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]);
+            Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]);
+            Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]);
+            Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
+            Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
+            Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
+            Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
+            Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
+            Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
+            Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
+            Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
+            Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
+            // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
+            Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
+            // Estimation of selected states is inhibited by setting their Kalman gains to zero
+            if (!inhibitWindStates) {
+                Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
+                Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
+            } else {
+                Kfusion[14] = 0;
+                Kfusion[15] = 0;
+            }
+            if (!inhibitMagStates) {
+                Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
+                Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
+                Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
+                Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
+                Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
+                Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
+            } else {
+                Kfusion[16] = 0;
+                Kfusion[17] = 0;
+                Kfusion[18] = 0;
+                Kfusion[19] = 0;
+                Kfusion[20] = 0;
+                Kfusion[21] = 0;
+            }
+            if (!inhibitGndHgtState) {
+                Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
+            } else {
+                Kfusion[22] = 0;
+            }
+            varInnovMag[1] = 1.0f/SK_MY[0];
+            innovMag[1] = MagPred[1] - magData.y;
+        }
+        else if (obsIndex == 2) // we are now fusing the Z measurement
+        {
+            // Calculate observation jacobians
+            for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[1];
+            H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
+            H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            H_MAG[3] = SH_MAG[0];
+            H_MAG[16] = 2*q0*q2 + 2*q1*q3;
+            H_MAG[17] = 2*q2*q3 - 2*q0*q1;
+            H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[21] = 1;
+
+            // Calculate Kalman gain
+            float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MZ[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[21][21] += 0.1f*R_MAG;
+                obsIndex = 3;
+                return;
+            }
+            SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+            SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
+            SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
+            Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]);
+            Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]);
+            Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]);
+            Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]);
+            Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]);
+            Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]);
+            Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]);
+            Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]);
+            Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]);
+            Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]);
+            Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]);
+            Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]);
+            Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]);
+            // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
+            Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]);
+            // Estimation of selected states is inhibited by setting their Kalman gains to zero
+            if (!inhibitWindStates) {
+                Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
+                Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
+            } else {
+                Kfusion[14] = 0;
+                Kfusion[15] = 0;
+            }
+            if (!inhibitMagStates) {
+                Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
+                Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
+                Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
+                Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
+                Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
+                Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
+            } else {
+                Kfusion[16] = 0;
+                Kfusion[17] = 0;
+                Kfusion[18] = 0;
+                Kfusion[19] = 0;
+                Kfusion[20] = 0;
+                Kfusion[21] = 0;
+            }
+            if (!inhibitGndHgtState) {
+                Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
+            } else {
+                Kfusion[22] = 0;
+            }
+            varInnovMag[2] = 1.0f/SK_MZ[0];
+            innovMag[2] = MagPred[2] - magData.z;
+
+        }
+
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
+        {
+            // correct the state vector
+            for (uint8_t j= 0; j < n_states; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12f)
+            {
+                for (uint8_t j= 0; j<=3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in KH to reduce the
+            // number of operations
+            for (uint8_t i = 0; i < n_states; i++)
+            {
+                for (uint8_t j = 0; j <= 3; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_MAG[j];
+                }
+                for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f;
+                if (!onGround)
+                {
+                    for (uint8_t j = 16; j <= 21; j++)
+                    {
+                        KH[i][j] = Kfusion[i] * H_MAG[j];
+                    }
+                }
+                else
+                {
+                    for (uint8_t j = 16; j <= 21; j++)
+                    {
+                        KH[i][j] = 0.0f;
+                    }
+                }
+            }
+            for (uint8_t i = 0; i < n_states; i++)
+            {
+                for (uint8_t j = 0; j < n_states; j++)
+                {
+                    KHP[i][j] = 0.0f;
+                    for (uint8_t k = 0; k <= 3; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    if (!onGround)
+                    {
+                        for (uint8_t k = 16; k<=21; k++)
+                        {
+                            KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                        }
+                    }
+                }
+            }
+        }
+        for (uint8_t i = 0; i < n_states; i++)
+        {
+            for (uint8_t j = 0; j < n_states; j++)
+            {
+                P[i][j] = P[i][j] - KHP[i][j];
+            }
+        }
+    }
+    obsIndex = obsIndex + 1;
+
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::FuseAirspeed()
+{
+    float vn;
+    float ve;
+    float vd;
+    float vwn;
+    float vwe;
+    float R_TAS = sq(airspeedMeasurementSigma);
+    float SH_TAS[3];
+    float SK_TAS;
+    float VtasPred;
+
+    // Copy required states to local variable names
+    vn = statesAtVtasMeasTime[4];
+    ve = statesAtVtasMeasTime[5];
+    vd = statesAtVtasMeasTime[6];
+    vwn = statesAtVtasMeasTime[14];
+    vwe = statesAtVtasMeasTime[15];
+
+    // Need to check that it is flying before fusing airspeed data
+    // Calculate the predicted airspeed
+    VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
+    // Perform fusion of True Airspeed measurement
+    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
+    {
+        // Calculate observation jacobians
+        SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
+        SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
+        SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
+
+        float H_TAS[n_states];
+        for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f;
+        H_TAS[4] = SH_TAS[2];
+        H_TAS[5] = SH_TAS[1];
+        H_TAS[6] = vd*SH_TAS[0];
+        H_TAS[14] = -SH_TAS[2];
+        H_TAS[15] = -SH_TAS[1];
+
+        // Calculate Kalman gains
+        float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
+        if (temp >= R_TAS) {
+            SK_TAS = 1.0f / temp;
+        } else {
+            // the calculation is badly conditioned, so we cannot perform fusion on this step
+            // we increase the wind state variances and try again next time
+            P[14][14] += 0.05f*R_TAS;
+            P[15][15] += 0.05f*R_TAS;
+            return;
+        }
+        Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
+        Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
+        Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
+        Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
+        Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
+        Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
+        Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
+        Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
+        Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
+        Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
+        Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
+        Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
+        Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
+        // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
+        Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
+        // Estimation of selected states is inhibited by setting their Kalman gains to zero
+        if (!inhibitWindStates) {
+            Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
+            Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
+        } else {
+            Kfusion[14] = 0;
+            Kfusion[15] = 0;
+        }
+        if (!inhibitMagStates) {
+            Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
+            Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
+            Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
+            Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
+            Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
+            Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
+        } else {
+            for (uint8_t i=16; i <= 21; i++) {
+                Kfusion[i] = 0;
+            }
+        }
+        if (!inhibitGndHgtState) {
+            Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
+        } else {
+            Kfusion[22] = 0;
+        }
+        varInnovVtas = 1.0f/SK_TAS;
+
+        // Calculate the measurement innovation
+        innovVtas = VtasPred - VtasMeas;
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovVtas*innovVtas*SK_TAS) < 25.0f)
+        {
+            // correct the state vector
+            for (uint8_t j=0; j <= 22; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovVtas;
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12f)
+            {
+                for (uint8_t j= 0; j <= 3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in H to reduce the
+            // number of operations
+            for (uint8_t i = 0; i <= 22; i++)
+            {
+                for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 4; j <= 6; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 7; j <= 13; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 14; j <= 15; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0;
+            }
+            for (uint8_t i = 0; i <= 22; i++)
+            {
+                for (uint8_t j = 0; j <= 22; j++)
+                {
+                    KHP[i][j] = 0.0;
+                    for (uint8_t k = 4; k <= 6; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    for (uint8_t k = 14; k <= 15; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                }
+            }
+            for (uint8_t i = 0; i <= 22; i++)
+            {
+                for (uint8_t j = 0; j <= 22; j++)
+                {
+                    P[i][j] = P[i][j] - KHP[i][j];
+                }
+            }
+        }
+    }
+
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+    uint8_t row;
+    uint8_t col;
+    for (row=first; row<=last; row++)
+    {
+        for (col=0; col<n_states; col++)
+        {
+            covMat[row][col] = 0.0;
+        }
+    }
+}
+
+void AttPosEKF::FuseRangeFinder()
+{
+
+    // Local variables
+    float rngPred;
+    float SH_RNG[5];
+    float H_RNG[23];
+    float SK_RNG[6];
+    float cosRngTilt;
+    const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance
+
+    // Copy required states to local variable names
+    float q0 = statesAtRngTime[0];
+    float q1 = statesAtRngTime[1];
+    float q2 = statesAtRngTime[2];
+    float q3 = statesAtRngTime[3];
+    float pd = statesAtRngTime[9];
+    float ptd = statesAtRngTime[22];
+
+    // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
+    SH_RNG[4] = sinf(rngFinderPitch);
+    cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
+    if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f)
+    {
+        // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
+        // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations
+        SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
+        SH_RNG[1] = pd - ptd;
+        SH_RNG[2] = 1/sq(SH_RNG[0]);
+        SH_RNG[3] = 1/SH_RNG[0];
+        for (uint8_t i = 0; i < n_states; i++) {
+            H_RNG[i] = 0.0f;
+            Kfusion[i] = 0.0f;
+        }
+        H_RNG[22] = -SH_RNG[3];
+        SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])));
+        SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4];
+        SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4];
+        SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4];
+        SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4];
+        SK_RNG[5] = SH_RNG[2];
+        Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]);
+
+        // Calculate the innovation variance for data logging
+        varInnovRng = 1.0f/SK_RNG[0];
+
+        // Calculate the measurement innovation
+        rngPred = (ptd - pd)/cosRngTilt;
+        innovRng = rngPred - rngMea;
+
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovRng*innovRng*SK_RNG[0]) < 25)
+        {
+            // correct the state vector
+            states[22] = states[22] - Kfusion[22] * innovRng;
+
+            // correct the covariance P = (I - K*H)*P
+            P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22];
+            P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f);
+        }
+    }
+
+}
+
+void AttPosEKF::FuseOptFlow()
+{
+    static uint8_t obsIndex;
+    static float SH_LOS[13];
+    static float SKK_LOS[15];
+    static float SK_LOS[2];
+    static float q0 = 0.0f;
+    static float q1 = 0.0f;
+    static float q2 = 0.0f;
+    static float q3 = 1.0f;
+    static float vn = 0.0f;
+    static float ve = 0.0f;
+    static float vd = 0.0f;
+    static float pd = 0.0f;
+    static float ptd = 0.0f;
+    static float R_LOS = 0.01f;
+    static float losPred[2];
+
+    // Transformation matrix from nav to body axes
+    Mat3f Tnb_local;
+    // Transformation matrix from body to sensor axes
+    // assume camera is aligned with Z body axis plus a misalignment
+    // defined by 3 small angles about X, Y and Z body axis
+    Mat3f Tbs;
+    Tbs.x.y =  a3;
+    Tbs.y.x = -a3;
+    Tbs.x.z = -a2;
+    Tbs.z.x =  a2;
+    Tbs.y.z =  a1;
+    Tbs.z.y = -a1;
+    // Transformation matrix from navigation to sensor axes
+    Mat3f Tns;
+    float H_LOS[n_states];
+    for (uint8_t i = 0; i < n_states; i++) {
+        H_LOS[i] = 0.0f;
+    }
+    Vector3f velNED_local;
+    Vector3f relVelSensor;
+
+// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
+    if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
+    {
+        // Sequential fusion of XY components to spread processing load across
+        // two prediction time steps.
+
+        // Calculate observation jacobians and Kalman gains
+        if (fuseOptFlowData)
+        {
+            // Copy required states to local variable names
+            q0       = statesAtOptFlowTime[0];
+            q1       = statesAtOptFlowTime[1];
+            q2       = statesAtOptFlowTime[2];
+            q3       = statesAtOptFlowTime[3];
+            vn       = statesAtOptFlowTime[4];
+            ve       = statesAtOptFlowTime[5];
+            vd       = statesAtOptFlowTime[6];
+            pd       = statesAtOptFlowTime[9];
+            ptd      = statesAtOptFlowTime[22];
+            velNED_local.x = vn;
+            velNED_local.y = ve;
+            velNED_local.z = vd;
+
+            // calculate rotation from NED to body axes
+            float q00 = sq(q0);
+            float q11 = sq(q1);
+            float q22 = sq(q2);
+            float q33 = sq(q3);
+            float q01 = q0 * q1;
+            float q02 = q0 * q2;
+            float q03 = q0 * q3;
+            float q12 = q1 * q2;
+            float q13 = q1 * q3;
+            float q23 = q2 * q3;
+            Tnb_local.x.x = q00 + q11 - q22 - q33;
+            Tnb_local.y.y = q00 - q11 + q22 - q33;
+            Tnb_local.z.z = q00 - q11 - q22 + q33;
+            Tnb_local.y.x = 2*(q12 - q03);
+            Tnb_local.z.x = 2*(q13 + q02);
+            Tnb_local.x.y = 2*(q12 + q03);
+            Tnb_local.z.y = 2*(q23 - q01);
+            Tnb_local.x.z = 2*(q13 - q02);
+            Tnb_local.y.z = 2*(q23 + q01);
+
+            // calculate transformation from NED to sensor axes
+            Tns = Tbs*Tnb_local;
+
+            // calculate range from ground plain to centre of sensor fov assuming flat earth
+            float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
+
+            // calculate relative velocity in sensor frame
+            relVelSensor = Tns*velNED_local;
+
+            // divide velocity by range  and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
+            losPred[0] =  relVelSensor.y/range;
+            losPred[1] = -relVelSensor.x/range;
+
+            //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
+            //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
+            //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
+
+            // Calculate observation jacobians
+            SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
+            SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
+            SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
+            SH_LOS[3] = 1/(pd - ptd);
+            SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
+            SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
+            SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
+            SH_LOS[7] = 1/sq(pd - ptd);
+            SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
+            SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
+            SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
+            SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
+            SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
+
+            for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
+            H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
+            H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
+            H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
+            H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
+            H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
+            H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
+            H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
+            H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
+            H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
+
+            // Calculate Kalman gain
+            SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
+            SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
+            SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
+            SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
+            SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
+            SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
+            SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
+            SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
+            SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
+            SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
+            SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
+            SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
+            SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
+            SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
+            SKK_LOS[14] = SH_LOS[0];
+
+            SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
+            Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
+            varInnovOptFlow[0] = 1.0f/SK_LOS[0];
+            innovOptFlow[0] = losPred[0] - losData[0];
+
+            // reset the observation index to 0 (we start by fusing the X
+            // measurement)
+            obsIndex = 0;
+            fuseOptFlowData = false;
+        }
+        else if (obsIndex == 1) // we are now fusing the Y measurement
+        {
+            // Calculate observation jacobians
+            for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
+            H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
+            H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
+            H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
+            H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
+            H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
+            H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
+            H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
+            H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
+            H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
+
+            // Calculate Kalman gains
+            SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
+            Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
+            varInnovOptFlow[1] = 1.0f/SK_LOS[1];
+            innovOptFlow[1] = losPred[1] - losData[1];
+        }
+
+        // Check the innovation for consistency and don't fuse if > 3Sigma
+        if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
+        {
+            // correct the state vector
+            for (uint8_t j = 0; j < n_states; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12f)
+            {
+                for (uint8_t j= 0; j<=3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in KH to reduce the
+            // number of operations
+            for (uint8_t i = 0; i < n_states; i++)
+            {
+                for (uint8_t j = 0; j <= 6; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_LOS[j];
+                }
+                for (uint8_t j = 7; j <= 8; j++)
+                {
+                    KH[i][j] = 0.0f;
+                }
+                KH[i][9] = Kfusion[i] * H_LOS[9];
+                for (uint8_t j = 10; j <= 21; j++)
+                {
+                    KH[i][j] = 0.0f;
+                }
+                KH[i][22] = Kfusion[i] * H_LOS[22];
+            }
+            for (uint8_t i = 0; i < n_states; i++)
+            {
+                for (uint8_t j = 0; j < n_states; j++)
+                {
+                    KHP[i][j] = 0.0f;
+                    for (uint8_t k = 0; k <= 6; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
+                    KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
+                }
+            }
+        }
+        for (uint8_t i = 0; i <  n_states; i++)
+        {
+            for (uint8_t j = 0; j <  n_states; j++)
+            {
+                P[i][j] = P[i][j] - KHP[i][j];
+            }
+        }
+    }
+    obsIndex = obsIndex + 1;
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+    uint8_t row;
+    uint8_t col;
+    for (col=first; col<=last; col++)
+    {
+        for (row=0; row < n_states; row++)
+        {
+            covMat[row][col] = 0.0;
+        }
+    }
+}
+
+float AttPosEKF::sq(float valIn)
+{
+    return valIn*valIn;
+}
+
+// Store states in a history array along with time stamp
+void AttPosEKF::StoreStates(uint64_t timestamp_ms)
+{
+    for (unsigned i=0; i<n_states; i++)
+        storedStates[i][storeIndex] = states[i];
+    statetimeStamp[storeIndex] = timestamp_ms;
+    storeIndex++;
+    if (storeIndex == data_buffer_size)
+        storeIndex = 0;
+}
+
+void AttPosEKF::ResetStoredStates()
+{
+    // reset all stored states
+    memset(&storedStates[0][0], 0, sizeof(storedStates));
+    memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
+
+    // reset store index to first
+    storeIndex = 0;
+
+    // overwrite all existing states
+    for (unsigned i = 0; i < n_states; i++) {
+        storedStates[i][storeIndex] = states[i];
+    }
+
+    statetimeStamp[storeIndex] = millis();
+
+    // increment to next storage index
+    storeIndex++;
+}
+
+// Output the state vector stored at the time that best matches that specified by msec
+int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
+{
+    int ret = 0;
+
+    int64_t bestTimeDelta = 200;
+    unsigned bestStoreIndex = 0;
+    for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++)
+    {
+        // Work around a GCC compiler bug - we know 64bit support on ARM is
+        // sketchy in GCC.
+        uint64_t timeDelta;
+
+        if (msec > statetimeStamp[storeIndexLocal]) {
+            timeDelta = msec - statetimeStamp[storeIndexLocal];
+        } else {
+            timeDelta = statetimeStamp[storeIndexLocal] - msec;
+        }
+
+        if (timeDelta < (uint64_t)bestTimeDelta)
+        {
+            bestStoreIndex = storeIndexLocal;
+            bestTimeDelta = timeDelta;
+        }
+    }
+    if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
+    {
+        for (unsigned i=0; i < n_states; i++) {
+            if (isfinite(storedStates[i][bestStoreIndex])) {
+                statesForFusion[i] = storedStates[i][bestStoreIndex];
+            } else if (isfinite(states[i])) {
+                statesForFusion[i] = states[i];
+            } else {
+                // There is not much we can do here, except reporting the error we just
+                // found.
+                ret++;
+            }
+        }
+    }
+    else // otherwise output current state
+    {
+        for (unsigned i = 0; i < n_states; i++) {
+            if (isfinite(states[i])) {
+                statesForFusion[i] = states[i];
+            } else {
+                ret++;
+            }
+        }
+    }
+
+    return ret;
+}
+
+void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
+{
+    // Calculate the nav to body cosine matrix
+    float q00 = sq(quat[0]);
+    float q11 = sq(quat[1]);
+    float q22 = sq(quat[2]);
+    float q33 = sq(quat[3]);
+    float q01 =  quat[0]*quat[1];
+    float q02 =  quat[0]*quat[2];
+    float q03 =  quat[0]*quat[3];
+    float q12 =  quat[1]*quat[2];
+    float q13 =  quat[1]*quat[3];
+    float q23 =  quat[2]*quat[3];
+
+    Tnb.x.x = q00 + q11 - q22 - q33;
+    Tnb.y.y = q00 - q11 + q22 - q33;
+    Tnb.z.z = q00 - q11 - q22 + q33;
+    Tnb.y.x = 2*(q12 - q03);
+    Tnb.z.x = 2*(q13 + q02);
+    Tnb.x.y = 2*(q12 + q03);
+    Tnb.z.y = 2*(q23 - q01);
+    Tnb.x.z = 2*(q13 - q02);
+    Tnb.y.z = 2*(q23 + q01);
+}
+
+void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4])
+{
+    // Calculate the body to nav cosine matrix
+    float q00 = sq(quat[0]);
+    float q11 = sq(quat[1]);
+    float q22 = sq(quat[2]);
+    float q33 = sq(quat[3]);
+    float q01 =  quat[0]*quat[1];
+    float q02 =  quat[0]*quat[2];
+    float q03 =  quat[0]*quat[3];
+    float q12 =  quat[1]*quat[2];
+    float q13 =  quat[1]*quat[3];
+    float q23 =  quat[2]*quat[3];
+
+    Tbn_ret.x.x = q00 + q11 - q22 - q33;
+    Tbn_ret.y.y = q00 - q11 + q22 - q33;
+    Tbn_ret.z.z = q00 - q11 - q22 + q33;
+    Tbn_ret.x.y = 2*(q12 - q03);
+    Tbn_ret.x.z = 2*(q13 + q02);
+    Tbn_ret.y.x = 2*(q12 + q03);
+    Tbn_ret.y.z = 2*(q23 - q01);
+    Tbn_ret.z.x = 2*(q13 - q02);
+    Tbn_ret.z.y = 2*(q23 + q01);
+}
+
+void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
+{
+    float u1 = cos(0.5f*eul[0]);
+    float u2 = cos(0.5f*eul[1]);
+    float u3 = cos(0.5f*eul[2]);
+    float u4 = sin(0.5f*eul[0]);
+    float u5 = sin(0.5f*eul[1]);
+    float u6 = sin(0.5f*eul[2]);
+    quat[0] = u1*u2*u3+u4*u5*u6;
+    quat[1] = u4*u2*u3-u1*u5*u6;
+    quat[2] = u1*u5*u3+u4*u2*u6;
+    quat[3] = u1*u2*u6-u4*u5*u3;
+}
+
+void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
+{
+    y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
+    y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
+    y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
+}
+
+void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
+{
+    velNED[0] = gpsGndSpd*cosf(gpsCourse);
+    velNED[1] = gpsGndSpd*sinf(gpsCourse);
+    velNED[2] = gpsVelD;
+}
+
+void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
+{
+    posNED[0] = earthRadius * (lat - latReference);
+    posNED[1] = earthRadius * cos(latReference) * (lon - lonReference);
+    posNED[2] = -(hgt - hgtReference);
+}
+
+void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
+{
+    lat = latRef + (double)posNED[0] * earthRadiusInv;
+    lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
+    hgt = hgtRef - posNED[2];
+}
+
+void AttPosEKF::OnGroundCheck()
+{
+    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
+    if (staticMode) {
+        staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
+    }
+    // don't update wind states if there is no airspeed measurement
+    if (onGround || !useAirspeed) {
+        inhibitWindStates = true;
+    } else {
+        inhibitWindStates =false;
+    }
+    // don't update magnetic field states if on ground or not using compass
+    if (onGround || !useCompass) {
+        inhibitMagStates = true;
+    } else {
+        inhibitMagStates = false;
+    }
+    // don't update terrain offset state if on ground
+    if (onGround) {
+        inhibitGndHgtState = true;
+    } else {
+        inhibitGndHgtState = false;
+    }
+}
+
+void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
+{
+    //Define Earth rotation vector in the NED navigation frame
+    omega.x  = earthRate*cosf(latitude);
+    omega.y  = 0.0f;
+    omega.z  = -earthRate*sinf(latitude);
+}
+
+void AttPosEKF::CovarianceInit()
+{
+    // Calculate the initial covariance matrix P
+    P[0][0]   = 0.25f * sq(1.0f*deg2rad);
+    P[1][1]   = 0.25f * sq(1.0f*deg2rad);
+    P[2][2]   = 0.25f * sq(1.0f*deg2rad);
+    P[3][3]   = 0.25f * sq(10.0f*deg2rad);
+    P[4][4]   = sq(0.7f);
+    P[5][5]   = P[4][4];
+    P[6][6]   = sq(0.7f);
+    P[7][7]   = sq(15.0f);
+    P[8][8]   = P[7][7];
+    P[9][9]   = sq(5.0f);
+    P[10][10] = sq(0.1f*deg2rad*dtIMU);
+    P[11][11] = P[10][10];
+    P[12][12] = P[10][10];
+    P[13][13] = sq(0.2f*dtIMU);
+    P[14][14] = sq(0.0f);
+    P[15][15]  = P[14][14];
+    P[16][16] = sq(0.02f);
+    P[17][17] = P[16][16];
+    P[18][18] = P[16][16];
+    P[19][19] = sq(0.02f);
+    P[20][20] = P[19][19];
+    P[21][21] = P[19][19];
+    P[22][22] = sq(0.5f);
+}
+
+float AttPosEKF::ConstrainFloat(float val, float min, float max)
+{
+    float ret;
+    if (val > max) {
+        ret = max;
+        ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val);
+    } else if (val < min) {
+        ret = min;
+        ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val);
+    } else {
+        ret = val;
+    }
+
+    if (!isfinite(val)) {
+        //ekf_debug("constrain: non-finite!");
+    }
+
+    return ret;
+}
+
+void AttPosEKF::ConstrainVariances()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // State vector:
+    // 0-3: quaternions (q0, q1, q2, q3)
+    // 4-6: Velocity - m/sec (North, East, Down)
+    // 7-9: Position - m (North, East, Down)
+    // 10-12: Delta Angle bias - rad (X,Y,Z)
+    // 13: Delta Velocity bias - m/s (Z)
+    // 14-15: Wind Vector  - m/sec (North,East)
+    // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
+    // 22: Terrain  offset - m
+
+    // Constrain quaternion variances
+    for (unsigned i = 0; i <= 3; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Constrain velocity variances
+    for (unsigned i = 4; i <= 6; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Constrain position variances
+    for (unsigned i = 7; i <= 9; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
+    }
+
+    // Constrain delta angle bias variances
+    for (unsigned i = 10; i <= 12; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU));
+    }
+
+    // Constrain delta velocity bias variance
+    P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU));
+
+    // Wind velocity variances
+    for (unsigned i = 14; i <= 15; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Earth magnetic field variances
+    for (unsigned i = 16; i <= 18; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Body magnetic field variances
+    for (unsigned i = 19; i <= 21; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Constrain terrain offset variance
+    P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f);
+}
+
+void AttPosEKF::ConstrainStates()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // State vector:
+    // 0-3: quaternions (q0, q1, q2, q3)
+    // 4-6: Velocity - m/sec (North, East, Down)
+    // 7-9: Position - m (North, East, Down)
+    // 10-12: Delta Angle bias - rad (X,Y,Z)
+    // 13: Delta Velocity bias - m/s (Z)
+    // 14-15: Wind Vector  - m/sec (North,East)
+    // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
+    // 22: Terrain  offset - m
+
+    // Constrain quaternion
+    for (unsigned i = 0; i <= 3; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Constrain velocities to what GPS can do for us
+    for (unsigned i = 4; i <= 6; i++) {
+        states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f);
+    }
+
+    // Constrain position to a reasonable vehicle range (in meters)
+    for (unsigned i = 7; i <= 8; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f);
+    }
+
+    // Constrain altitude
+    states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f);
+
+    // Angle bias limit - set to 8 degrees / sec
+    for (unsigned i = 10; i <= 12; i++) {
+        states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
+    }
+
+    // Constrain delta velocity bias
+    states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU);
+
+    // Wind velocity limits - assume 120 m/s max velocity
+    for (unsigned i = 14; i <= 15; i++) {
+        states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
+    }
+
+    // Earth magnetic field limits (in Gauss)
+    for (unsigned i = 16; i <= 18; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Body magnetic field variances (in Gauss).
+    // the max offset should be in this range.
+    for (unsigned i = 19; i <= 21; i++) {
+        states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
+    }
+
+    // Constrain terrain offset
+    states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f);
+
+}
+
+void AttPosEKF::ForceSymmetry()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // Force symmetry on the covariance matrix to prevent ill-conditioning
+    // of the matrix which would cause the filter to blow-up
+    for (unsigned i = 1; i < n_states; i++)
+    {
+        for (uint8_t j = 0; j < i; j++)
+        {
+            P[i][j] = 0.5f * (P[i][j] + P[j][i]);
+            P[j][i] = P[i][j];
+
+            if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) ||
+                (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) {
+                current_ekf_state.covariancesExcessive = true;
+                current_ekf_state.error |= true;
+                InitializeDynamic(velNED, magDeclination);
+                return;
+            }
+
+            float symmetric = 0.5f * (P[i][j] + P[j][i]);
+            P[i][j] = symmetric;
+            P[j][i] = symmetric;
+        }
+    }
+}
+
+bool AttPosEKF::GyroOffsetsDiverged()
+{
+    // Detect divergence by looking for rapid changes of the gyro offset
+    Vector3f current_bias;
+    current_bias.x = states[10];
+    current_bias.y = states[11];
+    current_bias.z = states[12];
+
+    Vector3f delta = current_bias - lastGyroOffset;
+    float delta_len = delta.length();
+    float delta_len_scaled = 0.0f;
+
+    // Protect against division by zero
+    if (delta_len > 0.0f) {
+        float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f);
+        delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU;
+    }
+
+    bool diverged = (delta_len_scaled > 1.0f);
+    lastGyroOffset = current_bias;
+    current_ekf_state.error |= diverged;
+    current_ekf_state.gyroOffsetsExcessive = diverged;
+
+    return diverged;
+}
+
+bool AttPosEKF::VelNEDDiverged()
+{
+    Vector3f current_vel;
+    current_vel.x = states[4];
+    current_vel.y = states[5];
+    current_vel.z = states[6];
+
+    Vector3f gps_vel;
+    gps_vel.x = velNED[0];
+    gps_vel.y = velNED[1];
+    gps_vel.z = velNED[2];
+
+    Vector3f delta = current_vel - gps_vel;
+    float delta_len = delta.length();
+
+    bool excessive = (delta_len > 20.0f);
+
+    current_ekf_state.error |= excessive;
+    current_ekf_state.velOffsetExcessive = excessive;
+
+    return excessive;
+}
+
+bool AttPosEKF::FilterHealthy()
+{
+    if (!statesInitialised) {
+        return false;
+    }
+
+    // XXX Check state vector for NaNs and ill-conditioning
+
+    // Check if any of the major inputs timed out
+    if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) {
+        return false;
+    }
+
+    // Nothing fired, return ok.
+    return true;
+}
+
+/**
+ * Reset the filter position states
+ *
+ * This resets the position to the last GPS measurement
+ * or to zero in case of static position.
+ */
+void AttPosEKF::ResetPosition(void)
+{
+    if (staticMode) {
+        states[7] = 0;
+        states[8] = 0;
+    } else if (GPSstatus >= GPS_FIX_3D) {
+
+        // reset the states from the GPS measurements
+        states[7] = posNE[0];
+        states[8] = posNE[1];
+    }
+}
+
+/**
+ * Reset the height state.
+ *
+ * This resets the height state with the last altitude measurements
+ */
+void AttPosEKF::ResetHeight(void)
+{
+    // write to the state vector
+    states[9]   = -hgtMea;
+}
+
+/**
+ * Reset the velocity state.
+ */
+void AttPosEKF::ResetVelocity(void)
+{
+    if (staticMode) {
+        states[4] = 0.0f;
+        states[5] = 0.0f;
+        states[6] = 0.0f;
+    } else if (GPSstatus >= GPS_FIX_3D) {
+
+        states[4]  = velNED[0]; // north velocity from last reading
+        states[5]  = velNED[1]; // east velocity from last reading
+        states[6]  = velNED[2]; // down velocity from last reading
+    }
+}
+
+bool AttPosEKF::StatesNaN() {
+    bool err = false;
+
+    // check all integrators
+    if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
+        current_ekf_state.angNaN = true;
+        ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
+        err = true;
+        goto out;
+    } // delta angles
+
+    if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
+        current_ekf_state.angNaN = true;
+        ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
+        err = true;
+        goto out;
+    } // delta angles
+
+    if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
+        current_ekf_state.summedDelVelNaN = true;
+        ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
+        err = true;
+        goto out;
+    } // delta velocities
+
+    // check all states and covariance matrices
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            if (!isfinite(KH[i][j])) {
+
+                current_ekf_state.KHNaN = true;
+                err = true;
+                ekf_debug("KH NaN");
+                goto out;
+            } //  intermediate result used for covariance updates
+
+            if (!isfinite(KHP[i][j])) {
+
+                current_ekf_state.KHPNaN = true;
+                err = true;
+                ekf_debug("KHP NaN");
+                goto out;
+            } // intermediate result used for covariance updates
+
+            if (!isfinite(P[i][j])) {
+
+                current_ekf_state.covarianceNaN = true;
+                err = true;
+                ekf_debug("P NaN");
+            } // covariance matrix
+        }
+
+        if (!isfinite(Kfusion[i])) {
+
+            current_ekf_state.kalmanGainsNaN = true;
+            ekf_debug("Kfusion NaN");
+            err = true;
+            goto out;
+        } // Kalman gains
+
+        if (!isfinite(states[i])) {
+
+            current_ekf_state.statesNaN = true;
+            ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);
+            err = true;
+            goto out;
+        } // state matrix
+    }
+
+out:
+    if (err) {
+        current_ekf_state.error |= true;
+    }
+
+    return err;
+
+}
+
+/**
+ * Check the filter inputs and bound its operational state
+ *
+ * This check will reset the filter states if required
+ * due to a failure of consistency or timeout checks.
+ * it should be run after the measurement data has been
+ * updated, but before any of the fusion steps are
+ * executed.
+ */
+int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
+{
+
+    // Store the old filter state
+    bool currStaticMode = staticMode;
+
+    // Limit reset rate to 5 Hz to allow the filter
+    // to settle
+    if (millis() - lastReset < 200) {
+        return 0;
+    }
+
+    if (ekfDiverged) {
+        ekfDiverged = false;
+    }
+
+    int ret = 0;
+
+    // Check if we're on ground - this also sets static mode.
+    OnGroundCheck();
+
+    // Reset the filter if the states went NaN
+    if (StatesNaN()) {
+        ekf_debug("re-initializing dynamic");
+
+        // Reset and fill error report
+	    InitializeDynamic(velNED, magDeclination);
+
+        ret = 1;
+    }
+
+    // Reset the filter if the IMU data is too old
+    if (dtIMU > 0.3f) {
+
+        current_ekf_state.imuTimeout = true;
+
+        // Fill error report
+        GetFilterState(&last_ekf_error);
+
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        // Timeout cleared with this reset
+        current_ekf_state.imuTimeout = false;
+
+        // that's all we can do here, return
+        ret = 2;
+    }
+
+    // Check if we switched between states
+    if (currStaticMode != staticMode) {
+        // Fill error report, but not setting error flag
+        GetFilterState(&last_ekf_error);
+
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        ret = 3;
+    }
+
+    // Reset the filter if gyro offsets are excessive
+    if (GyroOffsetsDiverged()) {
+
+        // Reset and fill error report
+        InitializeDynamic(velNED, magDeclination);
+
+        // that's all we can do here, return
+        ret = 4;
+    }
+
+    // Reset the filter if it diverges too far from GPS
+    if (VelNEDDiverged()) {
+
+        // Reset and fill error report
+        InitializeDynamic(velNED, magDeclination);
+
+        // that's all we can do here, return
+        ret = 5;
+    }
+
+    // The excessive covariance detection already
+    // reset the filter. Just need to report here.
+    if (last_ekf_error.covariancesExcessive) {
+        ret = 6;
+    }
+
+    if (ret) {
+        ekfDiverged = true;
+        lastReset = millis();
+
+        // This reads the last error and clears it
+        GetLastErrorState(last_error);
+    }
+
+    return ret;
+}
+
+void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2f(-ay, -az);
+    initialPitch = atan2f(ax, -az);
+
+    cosRoll = cosf(initialRoll);
+    sinRoll = sinf(initialRoll);
+    cosPitch = cosf(initialPitch);
+    sinPitch = sinf(initialPitch);
+
+    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+    magY = my * cosRoll - mz * sinRoll;
+
+    initialHdg = atan2f(-magY, magX);
+    /* true heading is the mag heading minus declination */
+    initialHdg += declination;
+
+    cosRoll = cosf(initialRoll * 0.5f);
+    sinRoll = sinf(initialRoll * 0.5f);
+
+    cosPitch = cosf(initialPitch * 0.5f);
+    sinPitch = sinf(initialPitch * 0.5f);
+
+    cosHeading = cosf(initialHdg * 0.5f);
+    sinHeading = sinf(initialHdg * 0.5f);
+
+    initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+
+    /* normalize */
+    float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]);
+
+    initQuat[0] /= norm;
+    initQuat[1] /= norm;
+    initQuat[2] /= norm;
+    initQuat[3] /= norm;
+}
+
+void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
+{
+    if (current_ekf_state.error) {
+        GetFilterState(&last_ekf_error);
+    }
+
+    ZeroVariables();
+
+    // Reset error states
+    current_ekf_state.error = false;
+    current_ekf_state.angNaN = false;
+    current_ekf_state.summedDelVelNaN = false;
+    current_ekf_state.KHNaN = false;
+    current_ekf_state.KHPNaN = false;
+    current_ekf_state.PNaN = false;
+    current_ekf_state.covarianceNaN = false;
+    current_ekf_state.kalmanGainsNaN = false;
+    current_ekf_state.statesNaN = false;
+
+    current_ekf_state.velHealth = true;
+    current_ekf_state.posHealth = true;
+    current_ekf_state.hgtHealth = true;
+    
+    current_ekf_state.velTimeout = false;
+    current_ekf_state.posTimeout = false;
+    current_ekf_state.hgtTimeout = false;
+
+    fuseVelData = false;
+    fusePosData = false;
+    fuseHgtData = false;
+    fuseMagData = false;
+    fuseVtasData = false;
+
+    // Fill variables with valid data
+    velNED[0] = initvelNED[0];
+    velNED[1] = initvelNED[1];
+    velNED[2] = initvelNED[2];
+    magDeclination = declination;
+
+    // Calculate initial filter quaternion states from raw measurements
+    float initQuat[4];
+    Vector3f initMagXYZ;
+    initMagXYZ   = magData - magBias;
+    AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat);
+
+    // Calculate initial Tbn matrix and rotate Mag measurements into NED
+    // to set initial NED magnetic field states
+    quat2Tbn(Tbn, initQuat);
+    Tnb = Tbn.transpose();
+    Vector3f initMagNED;
+    initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z;
+    initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z;
+    initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z;
+
+    magstate.q0 = initQuat[0];
+    magstate.q1 = initQuat[1];
+    magstate.q2 = initQuat[2];
+    magstate.q3 = initQuat[3];
+    magstate.magN = initMagNED.x;
+    magstate.magE = initMagNED.y;
+    magstate.magD = initMagNED.z;
+    magstate.magXbias = magBias.x;
+    magstate.magYbias = magBias.y;
+    magstate.magZbias = magBias.z;
+    magstate.R_MAG = sq(magMeasurementSigma);
+    magstate.DCM = Tbn;
+
+    // write to state vector
+    for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
+    for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
+    // positions:
+    states[7] = posNE[0];
+    states[8] = posNE[1];
+    states[9] = -hgtMea;
+    for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
+    states[16] = initMagNED.x; // Magnetic Field North
+    states[17] = initMagNED.y; // Magnetic Field East
+    states[18] = initMagNED.z; // Magnetic Field Down
+    states[19] = magBias.x; // Magnetic Field Bias X
+    states[20] = magBias.y; // Magnetic Field Bias Y
+    states[21] = magBias.z; // Magnetic Field Bias Z
+    states[22] = 0.0f; // terrain height
+
+    ResetVelocity();
+    ResetPosition();
+    ResetHeight();
+
+    statesInitialised = true;
+
+    // initialise the covariance matrix
+    CovarianceInit();
+
+    //Define Earth rotation vector in the NED navigation frame
+    calcEarthRateNED(earthRateNED, latRef);
+
+}
+
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
+{
+    // store initial lat,long and height
+    latRef = referenceLat;
+    lonRef = referenceLon;
+    hgtRef = referenceHgt;
+    refSet = true;
+
+    // we are at reference position, so measurement must be zero
+    posNE[0] = 0.0f;
+    posNE[1] = 0.0f;
+
+    // we are at an unknown, possibly non-zero altitude - so altitude
+    // is not reset (hgtMea)
+
+    // the baro offset must be this difference now
+    baroHgtOffset = baroHgt - referenceHgt;
+
+    InitializeDynamic(initvelNED, declination);
+}
+
+void AttPosEKF::ZeroVariables()
+{
+
+    // Initialize on-init initialized variables
+
+    storeIndex = 0;
+
+    // Do the data structure init
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            KH[i][j] = 0.0f; //  intermediate result used for covariance updates
+            KHP[i][j] = 0.0f; // intermediate result used for covariance updates
+            P[i][j] = 0.0f; // covariance matrix
+        }
+
+        Kfusion[i] = 0.0f; // Kalman gains
+        states[i] = 0.0f; // state matrix
+    }
+
+    correctedDelAng.zero();
+    summedDelAng.zero();
+    summedDelVel.zero();
+    dAngIMU.zero();
+    dVelIMU.zero();
+    lastGyroOffset.zero();
+
+    for (unsigned i = 0; i < data_buffer_size; i++) {
+
+        for (unsigned j = 0; j < n_states; j++) {
+            storedStates[j][i] = 0.0f;
+        }
+
+        statetimeStamp[i] = 0;
+    }
+
+    memset(&magstate, 0, sizeof(magstate));
+    magstate.q0 = 1.0f;
+    magstate.DCM.identity();
+
+    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
+
+}
+
+void AttPosEKF::GetFilterState(struct ekf_status_report *err)
+{
+
+    // Copy states
+    for (unsigned i = 0; i < n_states; i++) {
+        current_ekf_state.states[i] = states[i];
+    }
+    current_ekf_state.n_states = n_states;
+
+    memcpy(err, &current_ekf_state, sizeof(*err));
+
+    // err->velHealth = current_ekf_state.velHealth;
+    // err->posHealth = current_ekf_state.posHealth;
+    // err->hgtHealth = current_ekf_state.hgtHealth;
+    // err->velTimeout = current_ekf_state.velTimeout;
+    // err->posTimeout = current_ekf_state.posTimeout;
+    // err->hgtTimeout = current_ekf_state.hgtTimeout;
+}
+
+void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
+{
+    memcpy(last_error, &last_ekf_error, sizeof(*last_error));
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h
new file mode 100644
index 000000000..faa6735ca
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h
@@ -0,0 +1,316 @@
+#pragma once
+
+#include "estimator_utilities.h"
+
+const unsigned int n_states = 23;
+const unsigned int data_buffer_size = 50;
+
+class AttPosEKF {
+
+public:
+
+    AttPosEKF();
+    ~AttPosEKF();
+
+
+
+    /* ##############################################
+     *
+     *   M A I N    F I L T E R    P A R A M E T E R S
+     *
+     * ########################################### */
+
+    /*
+     * parameters are defined here and initialised in
+     * the InitialiseParameters() (which is just 20 lines down)
+     */
+
+    float covTimeStepMax; // maximum time allowed between covariance predictions
+    float covDelAngMax; // maximum delta angle between covariance predictions
+    float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+
+    float a1; // optical flow sensor misalgnment angle about X axis (rad)
+    float a2; // optical flow sensor misalgnment angle about Y axis (rad)
+    float a3; // optical flow sensor misalgnment angle about Z axis (rad)
+
+    float yawVarScale;
+    float windVelSigma;
+    float dAngBiasSigma;
+    float dVelBiasSigma;
+    float magEarthSigma;
+    float magBodySigma;
+    float gndHgtSigma;
+
+    float vneSigma;
+    float vdSigma;
+    float posNeSigma;
+    float posDSigma;
+    float magMeasurementSigma;
+    float airspeedMeasurementSigma;
+
+    float gyroProcessNoise;
+    float accelProcessNoise;
+
+    float EAS2TAS; // ratio f true to equivalent airspeed
+
+    void InitialiseParameters()
+    {
+        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+        EAS2TAS = 1.0f;
+        a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
+        a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
+        a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
+
+        yawVarScale = 1.0f;
+        windVelSigma = 0.1f;
+        dAngBiasSigma = 5.0e-7f;
+        dVelBiasSigma = 1e-4f;
+        magEarthSigma = 3.0e-4f;
+        magBodySigma  = 3.0e-4f;
+        gndHgtSigma  = 0.02f; // assume 2% terrain gradient 1-sigma
+
+        vneSigma = 0.2f;
+        vdSigma = 0.3f;
+        posNeSigma = 2.0f;
+        posDSigma = 2.0f;
+
+        magMeasurementSigma = 0.05;
+        airspeedMeasurementSigma = 1.4f;
+        gyroProcessNoise = 1.4544411e-2f;
+        accelProcessNoise = 0.5f;
+    }
+
+    struct mag_state_struct {
+        unsigned obsIndex;
+        float MagPred[3];
+        float SH_MAG[9];
+        float q0;
+        float q1;
+        float q2;
+        float q3;
+        float magN;
+        float magE;
+        float magD;
+        float magXbias;
+        float magYbias;
+        float magZbias;
+        float R_MAG;
+        Mat3f DCM;
+    };
+
+    struct mag_state_struct magstate;
+    struct mag_state_struct resetMagState;
+
+
+
+
+    // Global variables
+    float KH[n_states][n_states]; //  intermediate result used for covariance updates
+    float KHP[n_states][n_states]; // intermediate result used for covariance updates
+    float P[n_states][n_states]; // covariance matrix
+    float Kfusion[n_states]; // Kalman gains
+    float states[n_states]; // state matrix
+    float resetStates[n_states];
+    float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
+    uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
+
+    float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
+    float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
+    float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
+    float statesAtRngTime[n_states]; // filter states at the effective measurement time
+    float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
+
+    Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
+    Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
+    float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
+    Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
+    Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+    Vector3f lastGyroOffset;    // Last gyro offset
+    Vector3f delAngTotal;
+
+    Mat3f Tbn; // transformation matrix from body to NED coordinates
+    Mat3f Tnb; // transformation amtrix from NED to body coordinates
+
+    Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
+    Vector3f dVelIMU;
+    Vector3f dAngIMU;
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
+    float innovVelPos[6]; // innovation output
+    float varInnovVelPos[6]; // innovation variance output
+
+    float velNED[3]; // North, East, Down velocity obs (m/s)
+    float accelGPSNED[3];   // Acceleration predicted by GPS in earth frame
+    float posNE[2]; // North, East position obs (m)
+    float hgtMea; //  measured height (m)
+    float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude
+    float rngMea; // Ground distance
+
+    float innovMag[3]; // innovation output
+    float varInnovMag[3]; // innovation variance output
+    Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
+    float losData[2]; // optical flow LOS rate measurements (rad/sec)
+    float innovVtas; // innovation output
+    float innovRng; ///< Range finder innovation
+    float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
+    float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
+    float varInnovVtas; // innovation variance output
+    float varInnovRng; // range finder innovation variance
+    float VtasMeas; // true airspeed measurement (m/s)
+    float magDeclination;       ///< magnetic declination
+    double latRef; // WGS-84 latitude of reference point (rad)
+    double lonRef; // WGS-84 longitude of reference point (rad)
+    float hgtRef; // WGS-84 height of reference point (m)
+    bool refSet;                ///< flag to indicate if the reference position has been set
+    Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
+    unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+
+    // GPS input data variables
+    double gpsLat;
+    double gpsLon;
+    float gpsHgt;
+    uint8_t GPSstatus;
+
+    // Baro input
+    float baroHgt;
+
+    bool statesInitialised;
+
+    bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
+    bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
+    bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
+    bool fuseMagData; // boolean true when magnetometer data is to be fused
+    bool fuseVtasData; // boolean true when airspeed data is to be fused
+    bool fuseRngData;   ///< true when range data is fused
+    bool fuseOptFlowData; // true when optical flow data is fused
+
+    bool inhibitWindStates; // true when wind states and covariances are to remain constant
+    bool inhibitMagStates;  // true when magnetic field states and covariances are to remain constant
+    bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant
+
+    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
+    bool staticMode;    ///< boolean true if no position feedback is fused
+    bool useAirspeed;    ///< boolean true if airspeed data is being used
+    bool useCompass;    ///< boolean true if magnetometer data is being used
+    bool useRangeFinder;     ///< true when rangefinder is being used
+    bool useOpticalFlow; // true when optical flow data is being used
+
+    bool ekfDiverged;
+    uint64_t lastReset;
+
+    struct ekf_status_report current_ekf_state;
+    struct ekf_status_report last_ekf_error;
+
+    bool numericalProtection;
+
+    unsigned storeIndex;
+
+
+void  UpdateStrapdownEquationsNED();
+
+void CovariancePrediction(float dt);
+
+void FuseVelposNED();
+
+void FuseMagnetometer();
+
+void FuseAirspeed();
+
+void FuseRangeFinder();
+
+void FuseOptFlow();
+
+void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void quatNorm(float (&quatOut)[4], const float quatIn[4]);
+
+// store staes along with system time stamp in msces
+void StoreStates(uint64_t timestamp_ms);
+
+/**
+ * Recall the state vector.
+ *
+ * Recalls the vector stored at closest time to the one specified by msec
+ *
+ * @return zero on success, integer indicating the number of invalid states on failure.
+ *         Does only copy valid states, if the statesForFusion vector was initialized
+ *         correctly by the caller, the result can be safely used, but is a mixture
+ *         time-wise where valid states were updated and invalid remained at the old
+ *         value.
+ */
+int RecallStates(float *statesForFusion, uint64_t msec);
+
+void ResetStoredStates();
+
+void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
+
+void calcEarthRateNED(Vector3f &omega, float latitude);
+
+static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+
+static void quat2eul(float (&eul)[3], const float (&quat)[4]);
+
+static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+
+void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
+
+static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
+
+static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+
+static float sq(float valIn);
+
+void OnGroundCheck();
+
+void CovarianceInit();
+
+void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
+
+float ConstrainFloat(float val, float min, float max);
+
+void ConstrainVariances();
+
+void ConstrainStates();
+
+void ForceSymmetry();
+
+int CheckAndBound(struct ekf_status_report *last_error);
+
+void ResetPosition();
+
+void ResetVelocity();
+
+void ZeroVariables();
+
+void GetFilterState(struct ekf_status_report *state);
+
+void GetLastErrorState(struct ekf_status_report *last_error);
+
+bool StatesNaN();
+
+void InitializeDynamic(float (&initvelNED)[3], float declination);
+
+protected:
+
+bool FilterHealthy();
+
+bool GyroOffsetsDiverged();
+
+bool VelNEDDiverged();
+
+void ResetHeight(void);
+
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
+
+};
+
+uint32_t millis();
+
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
new file mode 100644
index 000000000..29a8c8d1e
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -0,0 +1,159 @@
+
+#include "estimator_utilities.h"
+
+// Define EKF_DEBUG here to enable the debug print calls
+// if the macro is not set, these will be completely
+// optimized out by the compiler.
+//#define EKF_DEBUG
+
+#ifdef EKF_DEBUG
+#include <stdio.h>
+#include <stdarg.h>
+
+static void
+ekf_debug_print(const char *fmt, va_list args)
+{
+    fprintf(stderr, "%s: ", "[ekf]");
+    vfprintf(stderr, fmt, args);
+
+    fprintf(stderr, "\n");
+}
+
+void
+ekf_debug(const char *fmt, ...)
+{
+    va_list args;
+
+    va_start(args, fmt);
+    ekf_debug_print(fmt, args);
+}
+
+#else
+
+void ekf_debug(const char *fmt, ...) { while(0){} }
+#endif
+
+float Vector3f::length(void) const
+{
+    return sqrt(x*x + y*y + z*z);
+}
+
+void Vector3f::zero(void)
+{
+    x = 0.0f;
+    y = 0.0f;
+    z = 0.0f;
+}
+
+Mat3f::Mat3f() {
+    identity();
+}
+
+void Mat3f::identity() {
+    x.x = 1.0f;
+    x.y = 0.0f;
+    x.z = 0.0f;
+
+    y.x = 0.0f;
+    y.y = 1.0f;
+    y.z = 0.0f;
+
+    z.x = 0.0f;
+    z.y = 0.0f;
+    z.z = 1.0f;
+}
+
+Mat3f Mat3f::transpose(void) const
+{
+    Mat3f ret = *this;
+    swap_var(ret.x.y, ret.y.x);
+    swap_var(ret.x.z, ret.z.x);
+    swap_var(ret.y.z, ret.z.y);
+    return ret;
+}
+
+// overload + operator to provide a vector addition
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x + vecIn2.x;
+    vecOut.y = vecIn1.y + vecIn2.y;
+    vecOut.z = vecIn1.z + vecIn2.z;
+    return vecOut;
+}
+
+// overload - operator to provide a vector subtraction
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x - vecIn2.x;
+    vecOut.y = vecIn1.y - vecIn2.y;
+    vecOut.z = vecIn1.z - vecIn2.z;
+    return vecOut;
+}
+
+// overload * operator to provide a matrix vector product
+Vector3f operator*( Mat3f matIn, Vector3f vecIn)
+{
+    Vector3f vecOut;
+    vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
+    vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
+    vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
+    return vecOut;
+}
+
+// overload * operator to provide a matrix product
+Mat3f operator*( Mat3f matIn1, Mat3f matIn2)
+{
+    Mat3f matOut;
+    matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
+    matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y;
+    matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z;
+
+    matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x;
+    matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y;
+    matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z;
+
+    matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x;
+    matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y;
+    matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z;
+
+    return matOut;
+}
+
+// overload % operator to provide a vector cross product
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
+    vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
+    vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
+    return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(Vector3f vecIn1, float sclIn1)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x * sclIn1;
+    vecOut.y = vecIn1.y * sclIn1;
+    vecOut.z = vecIn1.z * sclIn1;
+    return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(float sclIn1, Vector3f vecIn1)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x * sclIn1;
+    vecOut.y = vecIn1.y * sclIn1;
+    vecOut.z = vecIn1.z * sclIn1;
+    return vecOut;
+}
+
+void swap_var(float &d1, float &d2)
+{
+    float tmp = d1;
+    d1 = d2;
+    d2 = tmp;
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
new file mode 100644
index 000000000..6d1f47b68
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
@@ -0,0 +1,89 @@
+#include <math.h>
+#include <stdint.h>
+
+#pragma once
+
+#define GRAVITY_MSS 9.80665f
+#define deg2rad 0.017453292f
+#define rad2deg 57.295780f
+#define pi 3.141592657f
+#define earthRate 0.000072921f
+#define earthRadius 6378145.0
+#define earthRadiusInv  1.5678540e-7
+
+class Vector3f
+{
+private:
+public:
+    float x;
+    float y;
+    float z;
+
+    Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) :
+    x(a),
+    y(b),
+    z(c)
+    {}
+
+    float length(void) const;
+    void zero(void);
+};
+
+class Mat3f
+{
+private:
+public:
+    Vector3f x;
+    Vector3f y;
+    Vector3f z;
+
+    Mat3f();
+
+    void identity();
+    Mat3f transpose(void) const;
+};
+
+Vector3f operator*(float sclIn1, Vector3f vecIn1);
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*( Mat3f matIn, Vector3f vecIn);
+Mat3f operator*( Mat3f matIn1, Mat3f matIn2);
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*(Vector3f vecIn1, float sclIn1);
+
+void swap_var(float &d1, float &d2);
+
+enum GPS_FIX {
+    GPS_FIX_NOFIX = 0,
+    GPS_FIX_2D = 2,
+    GPS_FIX_3D = 3
+};
+
+struct ekf_status_report {
+    bool error;
+    bool velHealth;
+    bool posHealth;
+    bool hgtHealth;
+    bool velTimeout;
+    bool posTimeout;
+    bool hgtTimeout;
+    bool imuTimeout;
+    uint32_t velFailTime;
+    uint32_t posFailTime;
+    uint32_t hgtFailTime;
+    float states[32];
+    unsigned n_states;
+    bool angNaN;
+    bool summedDelVelNaN;
+    bool KHNaN;
+    bool KHPNaN;
+    bool PNaN;
+    bool covarianceNaN;
+    bool kalmanGainsNaN;
+    bool statesNaN;
+    bool gyroOffsetsExcessive;
+    bool covariancesExcessive;
+    bool velOffsetExcessive;
+};
+
+void ekf_debug(const char *fmt, ...);
diff --git a/src/modules/fixedwing_att_control/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index fd1a8724a..dc5220bf0 100644
--- a/src/modules/fixedwing_att_control/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 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
@@ -32,11 +32,12 @@
 ############################################################################
 
 #
-# Fixedwing Attitude Control application
+# Main EKF Attitude and Position Estimator
 #
 
-MODULE_COMMAND	= fixedwing_att_control
+MODULE_COMMAND	= ekf_att_pos_estimator
 
-SRCS		= fixedwing_att_control_main.c \
-		  fixedwing_att_control_att.c \
-		  fixedwing_att_control_rate.c
+SRCS		= ekf_att_pos_estimator_main.cpp \
+		  ekf_att_pos_estimator_params.c \
+		  estimator_23states.cpp \
+		  estimator_utilities.cpp
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
deleted file mode 100644
index 2aeca3a98..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+++ /dev/null
@@ -1,169 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *
- * 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 fixedwing_att_control_rate.c
- * Implementation of a fixed wing attitude controller.
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <uORB/uORB.h>
-
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <systemlib/param/param.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/systemlib.h>
-
-#include "fixedwing_att_control_att.h"
-
-
-struct fw_att_control_params {
-	float roll_p;
-	float rollrate_lim;
-	float pitch_p;
-	float pitchrate_lim;
-	float yawrate_lim;
-	float pitch_roll_compensation_p;
-};
-
-struct fw_pos_control_param_handles {
-	param_t roll_p;
-	param_t rollrate_lim;
-	param_t pitch_p;
-	param_t pitchrate_lim;
-	param_t yawrate_lim;
-	param_t pitch_roll_compensation_p;
-};
-
-
-
-/* Internal Prototypes */
-static int parameters_init(struct fw_pos_control_param_handles *h);
-static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p);
-
-static int parameters_init(struct fw_pos_control_param_handles *h)
-{
-	/* PID parameters */
-	h->roll_p 		=	param_find("FW_ROLL_P");
-	h->rollrate_lim =	param_find("FW_ROLLR_LIM");
-	h->pitch_p 		=	param_find("FW_PITCH_P");
-	h->pitchrate_lim =	param_find("FW_PITCHR_LIM");
-	h->yawrate_lim =	param_find("FW_YAWR_LIM");
-	h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP");
-
-	return OK;
-}
-
-static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p)
-{
-	param_get(h->roll_p, &(p->roll_p));
-	param_get(h->rollrate_lim, &(p->rollrate_lim));
-	param_get(h->pitch_p, &(p->pitch_p));
-	param_get(h->pitchrate_lim, &(p->pitchrate_lim));
-	param_get(h->yawrate_lim, &(p->yawrate_lim));
-	param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p));
-
-	return OK;
-}
-
-int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				   const struct vehicle_attitude_s *att,
-				   const float speed_body[],
-				   struct vehicle_rates_setpoint_s *rates_sp)
-{
-	static int counter = 0;
-	static bool initialized = false;
-
-	static struct fw_att_control_params p;
-	static struct fw_pos_control_param_handles h;
-
-	static PID_t roll_controller;
-	static PID_t pitch_controller;
-
-
-	if (!initialized) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
-		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
-		initialized = true;
-	}
-
-	/* load new parameters with lower rate */
-	if (counter % 100 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-		pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
-		pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
-	}
-
-	/* Roll (P) */
-	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
-
-
-	/* Pitch (P) */
-
-	/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
-	float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
-	/* set pitch plus feedforward roll compensation */
-	rates_sp->pitch = pid_calculate(&pitch_controller,
-					att_sp->pitch_body + pitch_sp_rollcompensation,
-					att->pitch, 0, 0);
-
-	/* Yaw (from coordinated turn constraint or lateral force) */
-	rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
-			/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
-
-//	printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
-
-	counter++;
-
-	return 0;
-}
-
-
-
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
deleted file mode 100644
index b6b4546c2..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ /dev/null
@@ -1,367 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: 	@author Thomas Gubler <thomasgubler@student.ethz.ch>
- *   			@author Doug Weibel <douglas.weibel@colorado.edu>
- *
- * 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 fixedwing_att_control.c
- * Implementation of a fixed wing attitude controller.
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.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/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/debug_key_value.h>
-#include <systemlib/param/param.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-
-#include "fixedwing_att_control_rate.h"
-#include "fixedwing_att_control_att.h"
-
-/* Prototypes */
-/**
- * Deamon management function.
- */
-__EXPORT int fixedwing_att_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of deamon.
- */
-int fixedwing_att_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/* Variables */
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-/* Main Thread */
-int fixedwing_att_control_thread_main(int argc, char *argv[])
-{
-	/* read arguments */
-	bool verbose = false;
-
-	for (int i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
-			verbose = true;
-		}
-	}
-
-	/* welcome user */
-	printf("[fixedwing att control] started\n");
-
-	/* declare and safely initialize all structs */
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_attitude_setpoint_s att_sp;
-	memset(&att_sp, 0, sizeof(att_sp));
-	struct vehicle_rates_setpoint_s rates_sp;
-	memset(&rates_sp, 0, sizeof(rates_sp));
-	struct vehicle_global_position_s global_pos;
-	memset(&global_pos, 0, sizeof(global_pos));
-	struct manual_control_setpoint_s manual_sp;
-	memset(&manual_sp, 0, sizeof(manual_sp));
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct vehicle_status_s vstatus;
-	memset(&vstatus, 0, sizeof(vstatus));
-
-	/* output structs */
-	struct actuator_controls_s actuators;
-	memset(&actuators, 0, sizeof(actuators));
-
-
-	/* publish actuator controls */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
-		actuators.control[i] = 0.0f;
-	}
-
-	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
-	orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-
-	/* subscribe */
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* Setup of loop */
-	float gyro[3] = {0.0f, 0.0f, 0.0f};
-	float speed_body[3] = {0.0f, 0.0f, 0.0f};
-	struct pollfd fds = { .fd = att_sub, .events = POLLIN };
-
-	while (!thread_should_exit) {
-		/* wait for a sensor update, check for exit condition every 500 ms */
-		poll(&fds, 1, 500);
-
-		/* Check if there is a new position measurement or  attitude setpoint */
-		bool pos_updated;
-		orb_check(global_pos_sub, &pos_updated);
-		bool att_sp_updated;
-		orb_check(att_sp_sub, &att_sp_updated);
-
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-
-		if (att_sp_updated)
-			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
-
-		if (pos_updated) {
-			orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
-
-			if (att.R_valid) {
-				speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
-				speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
-				speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
-
-			} else {
-				speed_body[0] = 0;
-				speed_body[1] = 0;
-				speed_body[2] = 0;
-
-				printf("FW ATT CONTROL: Did not get a valid R\n");
-			}
-		}
-
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
-		orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-		orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
-
-		gyro[0] = att.rollspeed;
-		gyro[1] = att.pitchspeed;
-		gyro[2] = att.yawspeed;
-
-		/* set manual setpoints if required */
-		if (control_mode.flag_control_manual_enabled) {
-			if (control_mode.flag_control_attitude_enabled) {
-
-				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-				if (vstatus.rc_signal_lost) {
-
-					/* put plane into loiter */
-					att_sp.roll_body = 0.3f;
-					att_sp.pitch_body = 0.0f;
-
-					/* limit throttle to 60 % of last value if sane */
-					if (isfinite(manual_sp.throttle) &&
-					    (manual_sp.throttle >= 0.0f) &&
-					    (manual_sp.throttle <= 1.0f)) {
-						att_sp.thrust = 0.6f * manual_sp.throttle;
-
-					} else {
-						att_sp.thrust = 0.0f;
-					}
-
-					att_sp.yaw_body = 0;
-
-					// XXX disable yaw control, loiter
-
-				} else {
-
-					att_sp.roll_body = manual_sp.roll;
-					att_sp.pitch_body = manual_sp.pitch;
-					att_sp.yaw_body = 0;
-					att_sp.thrust = manual_sp.throttle;
-				}
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				/* pass through flaps */
-				if (isfinite(manual_sp.flaps)) {
-					actuators.control[4] = manual_sp.flaps;
-
-				} else {
-					actuators.control[4] = 0.0f;
-				}
-
-			} else {
-				/* directly pass through values */
-				actuators.control[0] = manual_sp.roll;
-				/* positive pitch means negative actuator -> pull up */
-				actuators.control[1] = manual_sp.pitch;
-				actuators.control[2] = manual_sp.yaw;
-				actuators.control[3] = manual_sp.throttle;
-
-				if (isfinite(manual_sp.flaps)) {
-					actuators.control[4] = manual_sp.flaps;
-
-				} else {
-					actuators.control[4] = 0.0f;
-				}
-			}
-		}
-		
-		/* execute attitude control if requested */
-		if (control_mode.flag_control_attitude_enabled) {
-			/* attitude control */
-			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-			/* angular rate control */
-			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-			/* pass through throttle */
-			actuators.control[3] = att_sp.thrust;
-
-			/* set flaps to zero */
-			actuators.control[4] = 0.0f;
-
-		}
-
-		/* publish rates */
-		orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
-
-		/* sanity check and publish actuator outputs */
-		if (isfinite(actuators.control[0]) &&
-		    isfinite(actuators.control[1]) &&
-		    isfinite(actuators.control[2]) &&
-		    isfinite(actuators.control[3])) {
-			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		}
-	}
-
-	printf("[fixedwing_att_control] exiting, stopping all motors.\n");
-	thread_running = false;
-
-	/* kill all outputs */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
-		actuators.control[i] = 0.0f;
-
-	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-
-
-
-	close(att_sub);
-	close(actuator_pub);
-	close(rates_pub);
-
-	fflush(stdout);
-	exit(0);
-
-	return 0;
-
-}
-
-/* Startup Functions */
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int fixedwing_att_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("fixedwing_att_control already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("fixedwing_att_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 20,
-					 2048,
-					 fixedwing_att_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		thread_running = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			printf("\tfixedwing_att_control is running\n");
-
-		} else {
-			printf("\tfixedwing_att_control not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-
-
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
deleted file mode 100644
index cdab39edc..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+++ /dev/null
@@ -1,211 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *
- * 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 fixedwing_att_control_rate.c
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *
- * Implementation of a fixed wing attitude controller.
- *
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <uORB/uORB.h>
-
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <systemlib/param/param.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/systemlib.h>
-
-#include "fixedwing_att_control_rate.h"
-
-/*
- * Controller parameters, accessible via MAVLink
- *
- */
-// Roll control parameters
-PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
-PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
-PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
-PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f);   // Roll rate limit in radians/sec, applies to the roll controller
-PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
-PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
-
-//Pitch control parameters
-PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
-PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
-PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
-PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f);   // Pitch rate limit in radians/sec, applies to the pitch controller
-PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
-
-//Yaw control parameters					//XXX TODO this is copy paste, asign correct values
-PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
-PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f);   // Yaw rate limit in radians/sec
-
-/* feedforward compensation */
-PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f);	/**< throttle to pitch coupling feedforward */
-
-struct fw_rate_control_params {
-	float rollrate_p;
-	float rollrate_i;
-	float rollrate_awu;
-	float pitchrate_p;
-	float pitchrate_i;
-	float pitchrate_awu;
-	float yawrate_p;
-	float yawrate_i;
-	float yawrate_awu;
-	float pitch_thr_ff;
-};
-
-struct fw_rate_control_param_handles {
-	param_t rollrate_p;
-	param_t rollrate_i;
-	param_t rollrate_awu;
-	param_t pitchrate_p;
-	param_t pitchrate_i;
-	param_t pitchrate_awu;
-	param_t yawrate_p;
-	param_t yawrate_i;
-	param_t yawrate_awu;
-	param_t pitch_thr_ff;
-};
-
-
-
-/* Internal Prototypes */
-static int parameters_init(struct fw_rate_control_param_handles *h);
-static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p);
-
-static int parameters_init(struct fw_rate_control_param_handles *h)
-{
-	/* PID parameters */
-	h->rollrate_p 	 =	param_find("FW_ROLLR_P");   //TODO define rate params for fixed wing
-	h->rollrate_i 	 =	param_find("FW_ROLLR_I");
-	h->rollrate_awu  =	param_find("FW_ROLLR_AWU");
-
-	h->pitchrate_p 	 =	param_find("FW_PITCHR_P");
-	h->pitchrate_i 	 =	param_find("FW_PITCHR_I");
-	h->pitchrate_awu =	param_find("FW_PITCHR_AWU");
-
-	h->yawrate_p 	 =	param_find("FW_YAWR_P");
-	h->yawrate_i 	 =	param_find("FW_YAWR_I");
-	h->yawrate_awu   =	param_find("FW_YAWR_AWU");
-	h->pitch_thr_ff  =	param_find("FW_PITCH_THR_P");
-
-	return OK;
-}
-
-static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p)
-{
-	param_get(h->rollrate_p, &(p->rollrate_p));
-	param_get(h->rollrate_i, &(p->rollrate_i));
-	param_get(h->rollrate_awu, &(p->rollrate_awu));
-	param_get(h->pitchrate_p, &(p->pitchrate_p));
-	param_get(h->pitchrate_i, &(p->pitchrate_i));
-	param_get(h->pitchrate_awu, &(p->pitchrate_awu));
-	param_get(h->yawrate_p, &(p->yawrate_p));
-	param_get(h->yawrate_i, &(p->yawrate_i));
-	param_get(h->yawrate_awu, &(p->yawrate_awu));
-	param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
-
-	return OK;
-}
-
-int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-				const float rates[],
-				struct actuator_controls_s *actuators)
-{
-	static int counter = 0;
-	static bool initialized = false;
-
-	static struct fw_rate_control_params p;
-	static struct fw_rate_control_param_handles h;
-
-	static PID_t roll_rate_controller;
-	static PID_t pitch_rate_controller;
-	static PID_t yaw_rate_controller;
-
-	static uint64_t last_run = 0;
-	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
-
-	if (!initialized) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller
-		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
-		initialized = true;
-	}
-
-	/* load new parameters with lower rate */
-	if (counter % 100 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-		pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
-		pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
-		pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
-	}
-
-
-	/* roll rate (PI) */
-	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
-	/* pitch rate (PI) */
-	actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
-	/* yaw rate (PI) */
-	actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
-
-	counter++;
-
-	return 0;
-}
-
-
-
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h
deleted file mode 100644
index 500e3e197..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *
- *
- * 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 Fixed Wing Attitude Rate Control */
-
-#ifndef FIXEDWING_ATT_CONTROL_RATE_H_
-#define FIXEDWING_ATT_CONTROL_RATE_H_
-
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-
-int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-				const float rates[],
-				struct actuator_controls_s *actuators);
-
-#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index 6dc19df41..bbb39f20f 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
 	_vCmd(this, "V_CMD"),
 	_crMax(this, "CR_MAX"),
 	_attPoll(),
-	_lastPosCmd(),
+	_lastMissionCmd(),
 	_timeStamp(0)
 {
 	_attPoll.fd = _att.getHandle();
@@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update()
 	setDt(dt);
 
 	// store old position command before update if new command sent
-	if (_posCmd.updated()) {
-		_lastPosCmd = _posCmd.getData();
+	if (_missionCmd.updated()) {
+		_lastMissionCmd = _missionCmd.getData();
 	}
 
 	// check for new updates
@@ -159,7 +159,7 @@ void BlockMultiModeBacksideAutopilot::update()
 	if (_status.main_state == MAIN_STATE_AUTO) {
 		// TODO use vehicle_control_mode here?
 		// update guidance
-		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
+		_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
 	}
 
 	// XXX handle STABILIZED (loiter on spot) as well
@@ -174,15 +174,15 @@ void BlockMultiModeBacksideAutopilot::update()
 		// of control we will limit the velocity feedback between
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
-					_pos.vx * _pos.vx +
-					_pos.vy * _pos.vy +
-					_pos.vz * _pos.vz));
+					_pos.vel_n * _pos.vel_n +
+					_pos.vel_e * _pos.vel_e +
+					_pos.vel_d * _pos.vel_d));
 
 		// limit velocity command between min/max velocity
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
+		float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt);
 
 		// heading hold
 		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
@@ -229,16 +229,16 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[CH_RDR] = _manual.yaw;
 		_actuators.control[CH_THR] = _manual.throttle;
 
-	} else if (_status.main_state == MAIN_STATE_SEATBELT ||
-		_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
+	} else if (_status.main_state == MAIN_STATE_ALTCTL ||
+		_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
 
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		// for the purpose of control we will limit the velocity feedback between
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
-					_pos.vx * _pos.vx +
-					_pos.vy * _pos.vy +
-					_pos.vz * _pos.vz));
+					_pos.vel_n * _pos.vel_n +
+					_pos.vel_e * _pos.vel_e +
+					_pos.vel_d * _pos.vel_d));
 
 		// pitch channel -> rate of climb
 		// TODO, might want to put a gain on this, otherwise commanding
diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index 567efeb35..e1c85c261 100644
--- a/src/modules/fixedwing_backside/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -264,7 +264,7 @@ private:
 	BlockParamFloat _crMax;
 
 	struct pollfd _attPoll;
-	vehicle_global_position_set_triplet_s _lastPosCmd;
+	position_setpoint_triplet_s _lastMissionCmd;
 	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
 	uint64_t _timeStamp;
 public:
diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
deleted file mode 100644
index 73df3fb9e..000000000
--- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ /dev/null
@@ -1,479 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: 	@author Thomas Gubler <thomasgubler@student.ethz.ch>
- *   			@author Doug Weibel <douglas.weibel@colorado.edu>
- *
- * 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 fixedwing_pos_control.c
- * Implementation of a fixed wing attitude controller.
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.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/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/parameter_update.h>
-#include <systemlib/param/param.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-
-/*
- * Controller parameters, accessible via MAVLink
- *
- */
-PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f);
-PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f);
-PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value
-PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f);		// Radians per meter off track
-PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
-PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f);	// Roll angle limit in radians
-PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
-PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f);	/**< Pitch angle limit in radians per second */
-
-struct fw_pos_control_params {
-	float heading_p;
-	float headingr_p;
-	float headingr_i;
-	float headingr_lim;
-	float xtrack_p;
-	float altitude_p;
-	float roll_lim;
-	float pitch_lim;
-};
-
-struct fw_pos_control_param_handles {
-	param_t heading_p;
-	param_t headingr_p;
-	param_t headingr_i;
-	param_t headingr_lim;
-	param_t xtrack_p;
-	param_t altitude_p;
-	param_t roll_lim;
-	param_t pitch_lim;
-};
-
-
-struct planned_path_segments_s {
-	bool segment_type;
-	double start_lat;			// Start of line or center of arc
-	double start_lon;
-	double end_lat;
-	double end_lon;
-	float radius;				// Radius of arc
-	float arc_start_bearing;		// Bearing from center to start of arc
-	float arc_sweep;				// Angle (radians) swept out by arc around center.
-	// Positive for clockwise, negative for counter-clockwise
-};
-
-
-/* Prototypes */
-/* Internal Prototypes */
-static int parameters_init(struct fw_pos_control_param_handles *h);
-static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
-
-/**
- * Deamon management function.
- */
-__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of deamon.
- */
-int fixedwing_pos_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/* Variables */
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-
-/**
- * Parameter management
- */
-static int parameters_init(struct fw_pos_control_param_handles *h)
-{
-	/* PID parameters */
-	h->heading_p 		=	param_find("FW_HEAD_P");
-	h->headingr_p 		=	param_find("FW_HEADR_P");
-	h->headingr_i 		=	param_find("FW_HEADR_I");
-	h->headingr_lim 	=	param_find("FW_HEADR_LIM");
-	h->xtrack_p 		=	param_find("FW_XTRACK_P");
-	h->altitude_p 		=	param_find("FW_ALT_P");
-	h->roll_lim 		=	param_find("FW_ROLL_LIM");
-	h->pitch_lim 		=	param_find("FW_PITCH_LIM");
-
-	return OK;
-}
-
-static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
-{
-	param_get(h->heading_p, &(p->heading_p));
-	param_get(h->headingr_p, &(p->headingr_p));
-	param_get(h->headingr_i, &(p->headingr_i));
-	param_get(h->headingr_lim, &(p->headingr_lim));
-	param_get(h->xtrack_p, &(p->xtrack_p));
-	param_get(h->altitude_p, &(p->altitude_p));
-	param_get(h->roll_lim, &(p->roll_lim));
-	param_get(h->pitch_lim, &(p->pitch_lim));
-
-	return OK;
-}
-
-
-/* Main Thread */
-int fixedwing_pos_control_thread_main(int argc, char *argv[])
-{
-	/* read arguments */
-	bool verbose = false;
-
-	for (int i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
-			verbose = true;
-		}
-	}
-
-	/* welcome user */
-	printf("[fixedwing pos control] started\n");
-
-	/* declare and safely initialize all structs */
-	struct vehicle_global_position_s global_pos;
-	memset(&global_pos, 0, sizeof(global_pos));
-	struct vehicle_global_position_s start_pos;		// Temporary variable, replace with
-	memset(&start_pos, 0, sizeof(start_pos));		// previous waypoint when available
-	struct vehicle_global_position_setpoint_s global_setpoint;
-	memset(&global_setpoint, 0, sizeof(global_setpoint));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct crosstrack_error_s xtrack_err;
-	memset(&xtrack_err, 0, sizeof(xtrack_err));
-	struct parameter_update_s param_update;
-	memset(&param_update, 0, sizeof(param_update));
-
-	/* output structs */
-	struct vehicle_attitude_setpoint_s attitude_setpoint;
-	memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
-
-	/* publish attitude setpoint */
-	attitude_setpoint.roll_body = 0.0f;
-	attitude_setpoint.pitch_body = 0.0f;
-	attitude_setpoint.yaw_body = 0.0f;
-	attitude_setpoint.thrust = 0.0f;
-	orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
-
-	/* subscribe */
-	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int param_sub = orb_subscribe(ORB_ID(parameter_update));
-
-	/* Setup of loop */
-	struct pollfd fds[2] = {
-		{ .fd = param_sub, .events = POLLIN },
-		{ .fd = att_sub, .events = POLLIN }
-	};
-	bool global_sp_updated_set_once = false;
-
-	float psi_track = 0.0f;
-
-	int counter = 0;
-
-	struct fw_pos_control_params p;
-	struct fw_pos_control_param_handles h;
-
-	PID_t heading_controller;
-	PID_t heading_rate_controller;
-	PID_t offtrack_controller;
-	PID_t altitude_controller;
-
-	parameters_init(&h);
-	parameters_update(&h, &p);
-	pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit
-	pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
-	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
-	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value
-
-	/* error and performance monitoring */
-	perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
-	perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
-
-	while (!thread_should_exit) {
-		/* wait for a sensor update, check for exit condition every 500 ms */
-		int ret = poll(fds, 2, 500);
-
-		if (ret < 0) {
-			/* poll error, count it in perf */
-			perf_count(fw_err_perf);
-
-		} else if (ret == 0) {
-			/* no return value, ignore */
-		} else {
-
-			/* only update parameters if they changed */
-			if (fds[0].revents & POLLIN) {
-				/* read from param to clear updated flag */
-				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), param_sub, &update);
-
-				/* update parameters from storage */
-				parameters_update(&h, &p);
-				pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
-				pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
-				pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
-				pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
-			}
-
-			/* only run controller if attitude changed */
-			if (fds[1].revents & POLLIN) {
-
-
-				static uint64_t last_run = 0;
-				const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-				last_run = hrt_absolute_time();
-
-				/* check if there is a new position or setpoint */
-				bool pos_updated;
-				orb_check(global_pos_sub, &pos_updated);
-				bool global_sp_updated;
-				orb_check(global_setpoint_sub, &global_sp_updated);
-
-				/* load local copies */
-				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-
-				if (pos_updated) {
-					orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
-				}
-
-				if (global_sp_updated) {
-					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
-					start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
-					global_sp_updated_set_once = true;
-					psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
-							(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
-
-					printf("next wp direction: %0.4f\n", (double)psi_track);
-				}
-
-				/* Simple Horizontal Control */
-				if (global_sp_updated_set_once) {
-					//				if (counter % 100 == 0)
-					//					printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
-
-					/* calculate crosstrack error */
-					// Only the case of a straight line track following handled so far
-					int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
-										(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
-										(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
-
-					// XXX what is xtrack_err.past_end?
-					if (distance_res == OK /*&& !xtrack_err.past_end*/) {
-
-						float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
-
-						float psi_c = psi_track + delta_psi_c;
-						float psi_e = psi_c - att.yaw;
-
-						/* wrap difference back onto -pi..pi range */
-						psi_e = _wrap_pi(psi_e);
-
-						if (verbose) {
-							printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
-							printf("delta_psi_c %.4f ", (double)delta_psi_c);
-							printf("psi_c %.4f ", (double)psi_c);
-							printf("att.yaw %.4f ", (double)att.yaw);
-							printf("psi_e %.4f ", (double)psi_e);
-						}
-
-						/* calculate roll setpoint, do this artificially around zero */
-						float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
-						float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
-						float psi_rate_c = delta_psi_rate_c + psi_rate_track;
-
-						/* limit turn rate */
-						if (psi_rate_c > p.headingr_lim) {
-							psi_rate_c = p.headingr_lim;
-
-						} else if (psi_rate_c < -p.headingr_lim) {
-							psi_rate_c = -p.headingr_lim;
-						}
-
-						float psi_rate_e = psi_rate_c - att.yawspeed;
-
-						// XXX sanity check: Assume 10 m/s stall speed and no stall condition
-						float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-
-						if (ground_speed < 10.0f) {
-							ground_speed = 10.0f;
-						}
-
-						float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
-
-						attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
-
-						if (verbose) {
-							printf("psi_rate_c %.4f ", (double)psi_rate_c);
-							printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
-							printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
-						}
-
-						if (verbose && counter % 100 == 0)
-							printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
-
-					} else {
-						if (verbose && counter % 100 == 0)
-							printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
-					}
-
-					/* Very simple Altitude Control */
-					if (pos_updated) {
-
-						//TODO: take care of relative vs. ab. altitude
-						attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
-
-					}
-
-					// XXX need speed control
-					attitude_setpoint.thrust = 0.7f;
-
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
-
-					/* measure in what intervals the controller runs */
-					perf_count(fw_interval_perf);
-
-					counter++;
-
-				} else {
-					// XXX no setpoint, decent default needed (loiter?)
-				}
-			}
-		}
-	}
-
-	printf("[fixedwing_pos_control] exiting.\n");
-	thread_running = false;
-
-
-	close(attitude_setpoint_pub);
-
-	fflush(stdout);
-	exit(0);
-
-	return 0;
-
-}
-
-/* Startup Functions */
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int fixedwing_pos_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("fixedwing_pos_control already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("fixedwing_pos_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 20,
-					 2048,
-					 fixedwing_pos_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		thread_running = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			printf("\tfixedwing_pos_control is running\n");
-
-		} else {
-			printf("\tfixedwing_pos_control not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 60c902ce5..4cdba735a 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
+ *   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
@@ -37,6 +36,7 @@
  * Implementation of a generic attitude controller based on classic orthogonal PIDs.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  */
 
@@ -62,6 +62,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -97,13 +98,21 @@ public:
 	/**
 	 * Start the sensors task.
 	 *
-	 * @return		OK on success.
+	 * @return	OK on success.
 	 */
 	int		start();
 
+	/**
+	 * Task status
+	 *
+	 * @return	true if the mainloop is running
+	 */
+	bool		task_running() { return _task_running; }
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_running;			/**< if true, task is running in its mainloop */
 	int		_control_task;			/**< task handle for sensor task */
 
 	int		_att_sub;			/**< vehicle attitude subscription */
@@ -111,13 +120,15 @@ private:
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_vcontrol_mode_sub;			/**< vehicle status subscription */
+	int		_vcontrol_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_sub;			/**< notification of manual control updates */
+	int		_global_pos_sub;		/**< global position subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
+	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct accel_report				_accel;			/**< body frame accelerations */
@@ -126,17 +137,21 @@ private:
 	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
+	struct vehicle_global_position_s		_global_pos;		/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
+	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
 
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-	bool		_airspeed_valid;		/**< flag if the airspeed measurement is valid */
 
 	struct {
 		float tconst;
 		float p_p;
 		float p_d;
 		float p_i;
+		float p_ff;
 		float p_rmax_pos;
 		float p_rmax_neg;
 		float p_integrator_max;
@@ -144,17 +159,32 @@ private:
 		float r_p;
 		float r_d;
 		float r_i;
+		float r_ff;
 		float r_integrator_max;
 		float r_rmax;
 		float y_p;
 		float y_i;
 		float y_d;
+		float y_ff;
 		float y_roll_feedforward;
 		float y_integrator_max;
+		float y_coordinated_min_speed;
+		float y_rmax;
 
 		float airspeed_min;
 		float airspeed_trim;
 		float airspeed_max;
+
+		float trim_roll;
+		float trim_pitch;
+		float trim_yaw;
+		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+		float man_roll_max;						/**< Max Roll in rad */
+		float man_pitch_max;					/**< Max Pitch in rad */
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -163,6 +193,7 @@ private:
 		param_t p_p;
 		param_t p_d;
 		param_t p_i;
+		param_t p_ff;
 		param_t p_rmax_pos;
 		param_t p_rmax_neg;
 		param_t p_integrator_max;
@@ -170,17 +201,29 @@ private:
 		param_t r_p;
 		param_t r_d;
 		param_t r_i;
+		param_t r_ff;
 		param_t r_integrator_max;
 		param_t r_rmax;
 		param_t y_p;
 		param_t y_i;
 		param_t y_d;
+		param_t y_ff;
 		param_t y_roll_feedforward;
 		param_t y_integrator_max;
+		param_t y_coordinated_min_speed;
+		param_t y_rmax;
 
 		param_t airspeed_min;
 		param_t airspeed_trim;
 		param_t airspeed_max;
+
+		param_t trim_roll;
+		param_t trim_pitch;
+		param_t trim_yaw;
+		param_t rollsp_offset_deg;
+		param_t pitchsp_offset_deg;
+		param_t man_roll_max;
+		param_t man_pitch_max;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -214,7 +257,7 @@ private:
 	/**
 	 * Check for airspeed updates.
 	 */
-	bool		vehicle_airspeed_poll();
+	void		vehicle_airspeed_poll();
 
 	/**
 	 * Check for accel updates.
@@ -227,6 +270,11 @@ private:
 	void		vehicle_setpoint_poll();
 
 	/**
+	 * Check for global position updates.
+	 */
+	void		global_pos_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -234,7 +282,8 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
+
 };
 
 namespace att_control
@@ -246,12 +295,13 @@ namespace att_control
 #endif
 static const int ERROR = -1;
 
-FixedwingAttitudeControl	*g_control;
+FixedwingAttitudeControl	*g_control = nullptr;
 }
 
 FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_task_should_exit(false),
+	_task_running(false),
 	_control_task(-1),
 
 /* subscriptions */
@@ -261,43 +311,69 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_sub(-1),
+	_global_pos_sub(-1),
 
 /* publications */
 	_rate_sp_pub(-1),
-	_actuators_0_pub(-1),
 	_attitude_sp_pub(-1),
+	_actuators_0_pub(-1),
+	_actuators_1_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
+	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
 /* states */
-	_setpoint_valid(false),
-	_airspeed_valid(false)
+	_setpoint_valid(false)
 {
+	/* safely initialize structs */
+	_att = {};
+	_accel = {};
+	_att_sp = {};
+	_manual = {};
+	_airspeed = {};
+	_vcontrol_mode = {};
+	_actuators = {};
+	_actuators_airframe = {};
+	_global_pos = {};
+
+
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
-	_parameter_handles.p_p = param_find("FW_P_P");
-	_parameter_handles.p_d = param_find("FW_P_D");
-	_parameter_handles.p_i = param_find("FW_P_I");
+	_parameter_handles.p_p = param_find("FW_PR_P");
+	_parameter_handles.p_i = param_find("FW_PR_I");
+	_parameter_handles.p_ff = param_find("FW_PR_FF");
 	_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
 	_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
-	_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
+	_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
 	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
 
-	_parameter_handles.r_p = param_find("FW_R_P");
-	_parameter_handles.r_d = param_find("FW_R_D");
-	_parameter_handles.r_i = param_find("FW_R_I");
-	_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
+	_parameter_handles.r_p = param_find("FW_RR_P");
+	_parameter_handles.r_i = param_find("FW_RR_I");
+	_parameter_handles.r_ff = param_find("FW_RR_FF");
+	_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
 	_parameter_handles.r_rmax = param_find("FW_R_RMAX");
 
-	_parameter_handles.y_p = param_find("FW_Y_P");
-	_parameter_handles.y_i = param_find("FW_Y_I");
-	_parameter_handles.y_d = param_find("FW_Y_D");
-	_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
-	_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
+	_parameter_handles.y_p = param_find("FW_YR_P");
+	_parameter_handles.y_i = param_find("FW_YR_I");
+	_parameter_handles.y_ff = param_find("FW_YR_FF");
+	_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
+	_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
 
+	_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
+
+	_parameter_handles.trim_roll = param_find("TRIM_ROLL");
+	_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
+	_parameter_handles.trim_yaw = param_find("TRIM_YAW");
+	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
+	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
+
+	_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
+	_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -324,6 +400,10 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
 		} while (_control_task != -1);
 	}
 
+	perf_free(_loop_perf);
+	perf_free(_nonfinite_input_perf);
+	perf_free(_nonfinite_output_perf);
+
 	att_control::g_control = nullptr;
 }
 
@@ -333,53 +413,69 @@ FixedwingAttitudeControl::parameters_update()
 
 	param_get(_parameter_handles.tconst, &(_parameters.tconst));
 	param_get(_parameter_handles.p_p, &(_parameters.p_p));
-	param_get(_parameter_handles.p_d, &(_parameters.p_d));
 	param_get(_parameter_handles.p_i, &(_parameters.p_i));
+	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
 	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
 	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
 	param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
 
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
-	param_get(_parameter_handles.r_d, &(_parameters.r_d));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
+	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
+
 	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
 	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
 
 	param_get(_parameter_handles.y_p, &(_parameters.y_p));
 	param_get(_parameter_handles.y_i, &(_parameters.y_i));
-	param_get(_parameter_handles.y_d, &(_parameters.y_d));
-	param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
+	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
 	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
+	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
+	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
 
+	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
+	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
+	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
+	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
+	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
+	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
+	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+	param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
+	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
+	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
+	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
+
+
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
-	_pitch_ctrl.set_k_p(math::radians(_parameters.p_p));
-	_pitch_ctrl.set_k_i(math::radians(_parameters.p_i));
-	_pitch_ctrl.set_k_d(math::radians(_parameters.p_d));
-	_pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max));
+	_pitch_ctrl.set_k_p(_parameters.p_p);
+	_pitch_ctrl.set_k_i(_parameters.p_i);
+	_pitch_ctrl.set_k_ff(_parameters.p_ff);
+	_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
 	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
 	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
-	_pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward));
+	_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
 
 	/* roll control parameters */
 	_roll_ctrl.set_time_constant(_parameters.tconst);
-	_roll_ctrl.set_k_p(math::radians(_parameters.r_p));
-	_roll_ctrl.set_k_i(math::radians(_parameters.r_i));
-	_roll_ctrl.set_k_d(math::radians(_parameters.r_d));
-	_roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max));
+	_roll_ctrl.set_k_p(_parameters.r_p);
+	_roll_ctrl.set_k_i(_parameters.r_i);
+	_roll_ctrl.set_k_ff(_parameters.r_ff);
+	_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
 	_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
 
 	/* yaw control parameters */
-	_yaw_ctrl.set_k_side(math::radians(_parameters.y_p));
-	_yaw_ctrl.set_k_i(math::radians(_parameters.y_i));
-	_yaw_ctrl.set_k_d(math::radians(_parameters.y_d));
-	_yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward));
-	_yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max));
+	_yaw_ctrl.set_k_p(_parameters.y_p);
+	_yaw_ctrl.set_k_i(_parameters.y_i);
+	_yaw_ctrl.set_k_ff(_parameters.y_ff);
+	_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
+	_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
+	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
 	return OK;
 }
@@ -412,7 +508,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 	}
 }
 
-bool
+void
 FixedwingAttitudeControl::vehicle_airspeed_poll()
 {
 	/* check if there is a new position */
@@ -421,10 +517,8 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 
 	if (airspeed_updated) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-		return true;
+//		warnx("airspeed poll: ind: %.4f,  true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
 	}
-
-	return false;
 }
 
 void
@@ -453,6 +547,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll()
 }
 
 void
+FixedwingAttitudeControl::global_pos_poll()
+{
+	/* check if there is a new global position */
+	bool global_pos_updated;
+	orb_check(_global_pos_sub, &global_pos_updated);
+
+	if (global_pos_updated) {
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+	}
+}
+
+void
 FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
 	att_control::g_control->task_main();
@@ -476,6 +582,7 @@ FixedwingAttitudeControl::task_main()
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
@@ -485,7 +592,7 @@ FixedwingAttitudeControl::task_main()
 	parameters_update();
 
 	/* get an initial update for all sensor and status data */
-	(void)vehicle_airspeed_poll();
+	vehicle_airspeed_poll();
 	vehicle_setpoint_poll();
 	vehicle_accel_poll();
 	vehicle_control_mode_poll();
@@ -500,8 +607,12 @@ FixedwingAttitudeControl::task_main()
 	fds[1].fd = _att_sub;
 	fds[1].events = POLLIN;
 
+	_task_running = true;
+
 	while (!_task_should_exit) {
 
+		static int loop_counter = 0;
+
 		/* wait for up to 500ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
@@ -542,7 +653,7 @@ FixedwingAttitudeControl::task_main()
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
 
-			_airspeed_valid = vehicle_airspeed_poll();
+			vehicle_airspeed_poll();
 
 			vehicle_setpoint_poll();
 
@@ -552,6 +663,8 @@ FixedwingAttitudeControl::task_main()
 
 			vehicle_manual_poll();
 
+			global_pos_poll();
+
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
@@ -562,55 +675,82 @@ FixedwingAttitudeControl::task_main()
 				lock_integrator = true;
 			}
 
+			/* Simple handling of failsafe: deploy parachute if failsafe is on */
+			if (_vcontrol_mode.flag_control_termination_enabled) {
+				_actuators_airframe.control[1] = 1.0f;
+//				warnx("_actuators_airframe.control[1] = 1.0f;");
+			} else {
+				_actuators_airframe.control[1] = 0.0f;
+//				warnx("_actuators_airframe.control[1] = -1.0f;");
+			}
+
 			/* decide if in stabilized or full manual control */
 
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
 
-				/* scale from radians to normalized -1 .. 1 range */
-				const float actuator_scaling = 1.0f / (M_PI_F / 4.0f);
-
 				/* scale around tuning airspeed */
 
 				float airspeed;
 
-				/* if airspeed is smaller than min, the sensor is not giving good readings */
-				if (!_airspeed_valid ||
-				    (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) ||
-				    !isfinite(_airspeed.indicated_airspeed_m_s)) {
-					airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
-
+				/* if airspeed is not updating, we assume the normal average speed */
+				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
+				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+					airspeed = _parameters.airspeed_trim;
+					if (nonfinite) {
+						perf_count(_nonfinite_input_perf);
+					}
 				} else {
-					airspeed = _airspeed.indicated_airspeed_m_s;
+					airspeed = _airspeed.true_airspeed_m_s;
 				}
 
-				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
-				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
+				/*
+				 * For scaling our actuators using anything less than the min (close to stall)
+				 * speed doesn't make any sense - its the strongest reasonable deflection we
+				 * want to do in flight and its the baseline a human pilot would choose.
+				 *
+				 * Forcing the scaling to this value allows reasonable handheld tests.
+				 */
 
-				float roll_sp, pitch_sp;
+				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
+
+				float roll_sp = _parameters.rollsp_offset_rad;
+				float pitch_sp = _parameters.pitchsp_offset_rad;
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
-					roll_sp = _att_sp.roll_body;
-					pitch_sp = _att_sp.pitch_body;
+					/* read in attitude setpoint from attitude setpoint uorb topic */
+					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
-					if (_att_sp.roll_reset_integral)
+					if (_att_sp.roll_reset_integral) {
 						_roll_ctrl.reset_integrator();
-
+					}
+					if (_att_sp.pitch_reset_integral) {
+						_pitch_ctrl.reset_integrator();
+					}
+					if (_att_sp.yaw_reset_integral) {
+						_yaw_ctrl.reset_integrator();
+					}
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians
-					 * and a typical remote can only do 45 degrees, the mapping is
-					 * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians.
+					 * and a typical remote can only do around 45 degrees, the mapping is
+					 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
 					 *
 					 * With this mapping the stick angle is a 1:1 representation of
-					 * the commanded attitude. If more than 45 degrees are desired,
-					 * a scaling parameter can be applied to the remote.
+					 * the commanded attitude.
+					 *
+					 * The trim gets subtracted here from the manual setpoint to get
+					 * the intended attitude setpoint. Later, after the rate control step the
+					 * trim is added again to the control signal.
 					 */
-					roll_sp = _manual.roll * 0.75f;
-					pitch_sp = _manual.pitch * 0.75f;
-					throttle_sp = _manual.throttle;
+					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+						+ _parameters.rollsp_offset_rad;
+					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+						+ _parameters.pitchsp_offset_rad;
+					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
 					/*
@@ -622,7 +762,7 @@ FixedwingAttitudeControl::task_main()
 					att_sp.timestamp = hrt_absolute_time();
 					att_sp.roll_body = roll_sp;
 					att_sp.pitch_body = pitch_sp;
-					att_sp.yaw_body = 0.0f;
+					att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
 					att_sp.thrust = throttle_sp;
 
 					/* lazily publish the setpoint only once available */
@@ -636,54 +776,120 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
+				/* Prepare speed_body_u and speed_body_w */
+				float speed_body_u = 0.0f;
+				float speed_body_v = 0.0f;
+				float speed_body_w = 0.0f;
+				if(_att.R_valid) 	{
+					speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
+					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
+					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
+				} else	{
+					if (loop_counter % 10 == 0) {
+						warnx("Did not get a valid R\n");
+					}
+				}
+
+				/* Run attitude controllers */
 				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+					_roll_ctrl.control_attitude(roll_sp, _att.roll);
+					_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+					_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
+							speed_body_u, speed_body_v, speed_body_w,
+							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+
+					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
+					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
+							_att.rollspeed, _att.yawspeed,
+							_yaw_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
+					if (!isfinite(roll_u)) {
+						_roll_ctrl.reset_integrator();
+						perf_count(_nonfinite_output_perf);
 
-					float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
-									    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+						if (loop_counter % 10 == 0) {
+							warnx("roll_u %.4f", (double)roll_u);
+						}
+					}
 
-					float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
-									      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
+							_yaw_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
+					if (!isfinite(pitch_u)) {
+						_pitch_ctrl.reset_integrator();
+						perf_count(_nonfinite_output_perf);
+						if (loop_counter % 10 == 0) {
+							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+								" airspeed %.4f, airspeed_scaling %.4f,"
+								" roll_sp %.4f, pitch_sp %.4f,"
+								" _roll_ctrl.get_desired_rate() %.4f,"
+								" _pitch_ctrl.get_desired_rate() %.4f"
+								" att_sp.roll_body %.4f",
+								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
+								(double)airspeed, (double)airspeed_scaling,
+								(double)roll_sp, (double)pitch_sp,
+								(double)_roll_ctrl.get_desired_rate(),
+								(double)_pitch_ctrl.get_desired_rate(),
+								(double)_att_sp.roll_body);
+						}
+					}
 
-					float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
-									  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
+							_pitch_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+					if (!isfinite(yaw_u)) {
+						_yaw_ctrl.reset_integrator();
+						perf_count(_nonfinite_output_perf);
+						if (loop_counter % 10 == 0) {
+							warnx("yaw_u %.4f", (double)yaw_u);
+						}
+					}
 
 					/* throttle passed through */
 					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+					if (!isfinite(throttle_sp)) {
+						if (loop_counter % 10 == 0) {
+							warnx("throttle_sp %.4f", (double)throttle_sp);
+						}
+					}
+				} else {
+					perf_count(_nonfinite_input_perf);
+					if (loop_counter % 10 == 0) {
+						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
+					}
+				}
 
-					// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
-					// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
-					// 			_actuators.control[2], _actuators.control[3]);
-
-					/*
-					 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
-					 * only once available
-					 */
-					vehicle_rates_setpoint_s rates_sp;
-					rates_sp.roll = _roll_ctrl.get_desired_rate();
-					rates_sp.pitch = _pitch_ctrl.get_desired_rate();
-					rates_sp.yaw = 0.0f; // XXX not yet implemented
+				/*
+				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
+				 * only once available
+				 */
+				vehicle_rates_setpoint_s rates_sp;
+				rates_sp.roll = _roll_ctrl.get_desired_rate();
+				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+				rates_sp.yaw = _yaw_ctrl.get_desired_rate();
 
-					rates_sp.timestamp = hrt_absolute_time();
+				rates_sp.timestamp = hrt_absolute_time();
 
-					if (_rate_sp_pub > 0) {
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+				if (_rate_sp_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
 
-					} else {
-						/* advertise and publish */
-						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-					}
+				} else {
+					/* advertise and publish */
+					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 				}
 
 			} else {
 				/* manual/direct control */
-				_actuators.control[0] = _manual.roll;
-				_actuators.control[1] = _manual.pitch;
-				_actuators.control[2] = _manual.yaw;
-				_actuators.control[3] = _manual.throttle;
+				_actuators.control[0] = _manual.y;
+				_actuators.control[1] = -_manual.x;
+				_actuators.control[2] = _manual.r;
+				_actuators.control[3] = _manual.z;
 				_actuators.control[4] = _manual.flaps;
 			}
 
@@ -693,6 +899,7 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp = hrt_absolute_time();
 
 			if (_actuators_0_pub > 0) {
 				/* publish the attitude setpoint */
@@ -703,14 +910,29 @@ FixedwingAttitudeControl::task_main()
 				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
 			}
 
+			if (_actuators_1_pub > 0) {
+				/* publish the attitude setpoint */
+				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
+//				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
+//						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
+//						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
+//						(double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
+
+			} else {
+				/* advertise and publish */
+				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+			}
+
 		}
 
+		loop_counter++;
 		perf_end(_loop_perf);
 	}
 
 	warnx("exiting.\n");
 
 	_control_task = -1;
+	_task_running = false;
 	_exit(0);
 }
 
@@ -756,6 +978,14 @@ int fw_att_control_main(int argc, char *argv[])
 			err(1, "start failed");
 		}
 
+		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
+		while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
+			usleep(50000);
+			printf(".");
+			fflush(stdout);
+		}
+		printf("\n");
+
 		exit(0);
 	}
 
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index be76524da..7cae84678 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+f *   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
@@ -38,99 +38,334 @@
  * Parameters defined by the fixed-wing attitude control task
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
 
 #include <systemlib/param/param.h>
 
+
 /*
  * Controller parameters, accessible via MAVLink
  *
  */
 
-// @DisplayName		Attitude Time Constant
-// @Description		This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
-// @Range		0.4 to 1.0 seconds, in tens of seconds
+/**
+ * Attitude Time Constant
+ *
+ * This defines the latency between a step input and the achieved setpoint
+ * (inverse to a P gain). Half a second is a good start value and fits for
+ * most average systems. Smaller systems may require smaller values, but as
+ * this will wear out servos faster, the value should only be decreased as
+ * needed.
+ *
+ * @unit seconds
+ * @min 0.4
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
 
-// @DisplayName		Proportional gain.
-// @Description		This defines how much the elevator input will be commanded dependend on the current pitch error.
-// @Range		10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_P_P, 40.0f);
-
-// @DisplayName		Damping gain.
-// @Description		This gain damps the airframe pitch rate. In particular relevant for flying wings.
-// @Range		0.0 to 10.0, 0.1 increments
-PARAM_DEFINE_FLOAT(FW_P_D, 0.0f);
+/**
+ * Pitch rate proportional gain.
+ *
+ * This defines how much the elevator input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
 
-// @DisplayName		Integrator gain.
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0 to 50.0
-PARAM_DEFINE_FLOAT(FW_P_I, 0.0f);
+/**
+ * Pitch rate integrator gain.
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
 
-// @DisplayName		Maximum positive / up pitch rate.
-// @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds, in 1 increments
+/**
+ * Maximum positive / up pitch rate.
+ *
+ * This limits the maximum pitch up angular rate the controller will output (in
+ * degrees per second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
 
-// @DisplayName		Maximum negative / down pitch rate.
-// @Description		This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds, in 1 increments
+/**
+ * Maximum negative / down pitch rate.
+ *
+ * This limits the maximum pitch down up angular rate the controller will
+ * output (in degrees per second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
 
-// @DisplayName		Pitch Integrator Anti-Windup
-// @Description		This limits the range in degrees the integrator can wind up to.
-// @Range		0.0 to 45.0
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
-
-// @DisplayName		Roll feedforward gain.
-// @Description		This compensates during turns and ensures the nose stays level.
-// @Range		0.5 2.0
-// @Increment		0.05
-// @User		User
-PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
-
-// @DisplayName		Proportional Gain.
-// @Description		This gain controls the roll angle to roll actuator output.
-// @Range		10.0 200.0
-// @Increment		10.0
-// @User		User
-PARAM_DEFINE_FLOAT(FW_R_P, 40.0f);
-
-// @DisplayName		Damping Gain
-// @Description		Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
-// @Range		0.0 10.0
-// @Increment		1.0
-// @User		User
-PARAM_DEFINE_FLOAT(FW_R_D, 0.0f);
-
-// @DisplayName		Integrator Gain
-// @Description		This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
-// @Range		0.0 100.0
-// @Increment		5.0
-// @User		User
-PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
-
-// @DisplayName		Roll Integrator Anti-Windup
-// @Description		This limits the range in degrees the integrator can wind up to.
-// @Range		0.0 to 45.0
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
-
-// @DisplayName		Maximum Roll Rate
-// @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
-
-
-PARAM_DEFINE_FLOAT(FW_Y_P, 0);
-PARAM_DEFINE_FLOAT(FW_Y_I, 0);
-PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
-PARAM_DEFINE_FLOAT(FW_Y_D, 0);
-PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1);
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
+/**
+ * Pitch rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
+
+/**
+ * Roll to Pitch feedforward gain.
+ *
+ * This compensates during turns and ensures the nose stays level.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
+
+/**
+ * Roll rate proportional Gain
+ *
+ * This defines how much the aileron input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
+
+/**
+ * Roll rate integrator Gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
+
+/**
+ * Roll Integrator Anti-Windup
+ *
+ * The portion of the integrator part in the control surface deflection is limited to this value.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
+
+/**
+ * Maximum Roll Rate
+ *
+ * This limits the maximum roll rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
+
+/**
+ * Yaw rate proportional gain
+ *
+ * This defines how much the rudder input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
+
+/**
+ * Yaw rate integrator gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
+
+/**
+ * Yaw rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
+
+/**
+ * Maximum Yaw Rate
+ *
+ * This limits the maximum yaw rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
+
+/**
+ * Roll rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
+
+/**
+ * Pitch rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
+
+/**
+ * Yaw rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
+
+/**
+ * Minimal speed for yaw coordination
+ *
+ * For airspeeds above this value, the yaw rate is calculated for a coordinated
+ * turn. Set to a very high value to disable.
+ *
+ * @unit m/s
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
+
+/* Airspeed parameters:
+ * The following parameters about airspeed are used by the attitude and the
+ * position controller.
+ * */
+
+/**
+ * Minimum Airspeed
+ *
+ * If the airspeed falls below this value, the TECS controller will try to
+ * increase airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
+
+/**
+ * Trim Airspeed
+ *
+ * The TECS controller tries to fly at this airspeed.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
+
+/**
+ * Maximum Airspeed
+ *
+ * If the airspeed is above this value, the TECS controller will try to decrease
+ * airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
+
+/**
+ * Roll Setpoint Offset
+ *
+ * An airframe specific offset of the roll setpoint in degrees, the value is
+ * added to the roll setpoint and should correspond to the typical cruise speed
+ * of the airframe.
+ *
+ * @unit deg
+ * @min -90.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
+
+/**
+ * Pitch Setpoint Offset
+ *
+ * An airframe specific offset of the pitch setpoint in degrees, the value is
+ * added to the pitch setpoint and should correspond to the typical cruise
+ * speed of the airframe.
+ *
+ * @unit deg
+ * @min -90.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
+
+/**
+ * Max Manual Roll
+ *
+ * Max roll for manual control in attitude stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
+
+/**
+ * Max Manual Pitch
+ *
+ * Max pitch for manual control in attitude stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk
index 1e600757e..89c6494c5 100644
--- a/src/modules/fw_att_control/module.mk
+++ b/src/modules/fw_att_control/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	= fw_att_control
 
 SRCS		= fw_att_control_main.cpp \
 		  fw_att_control_params.c
+
+MODULE_STACKSIZE = 1200
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 a9648b207..08c996ebc 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
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
+ *   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
@@ -43,12 +42,13 @@
  *    Proceedings of the AIAA Guidance, Navigation and Control
  *    Conference, Aug 2004. AIAA-2004-4900.
  *
- * Original implementation for total energy control class:
- *    Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
+ * Implementation for total energy control class:
+ *    Thomas Gubler
  *
  * More details and acknowledgements in the referenced library headers.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -67,7 +67,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
@@ -75,6 +75,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -83,9 +84,13 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
-
+#include <mavlink/mavlink_log.h>
+#include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
-#include <external_lgpl/tecs/tecs.h>
+#include <drivers/drv_range_finder.h>
+#include "landingslope.h"
+#include "mtecs/mTecs.h"
+
 
 /**
  * L1 control app start / stop handling function
@@ -114,66 +119,94 @@ public:
 	 */
 	int		start();
 
+	/**
+	 * Task status
+	 *
+	 * @return	true if the mainloop is running
+	 */
+	bool		task_running() { return _task_running; }
+
 private:
+	int		_mavlink_fd;
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_running;			/**< if true, task is running in its mainloop */
 	int		_control_task;			/**< task handle for sensor task */
 
 	int		_global_pos_sub;
-	int		_global_set_triplet_sub;
+	int		_pos_sp_triplet_sub;
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_control_mode_sub;			/**< vehicle status subscription */
+	int		_control_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_accel_sub;			/**< body frame accelerations */
+	int		_sensor_combined_sub;		/**< for body frame accelerations */
+	int		_range_finder_sub;		/**< range finder subscription */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
-	struct navigation_capabilities_s		_nav_capabilities;	/**< navigation capabilities */
-	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct vehicle_global_position_set_triplet_s	_global_triplet;	/**< triplet of global setpoints */
-	struct accel_report				_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
+	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
+	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
+	struct airspeed_s				_airspeed;			/**< airspeed */
+	struct vehicle_control_mode_s			_control_mode;			/**< vehicle status */
+	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
+	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
+	struct range_finder_report 			_range_finder;			/**< range finder report */
 
 	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 */
-	float		_loiter_hold_lat;
-	float		_loiter_hold_lon;
+	float		_altctrl_hold_heading;		/**< heading the system should hold in altctrl mode */
+	double		_loiter_hold_lat;
+	double		_loiter_hold_lon;
 	float		_loiter_hold_alt;
 	bool		_loiter_hold;
 
-	float		_launch_lat;
-	float		_launch_lon;
+	double		_launch_lat;
+	double		_launch_lon;
 	float		_launch_alt;
 	bool		_launch_valid;
 
 	/* land states */
 	/* not in non-abort mode for landing yet */
-	bool land_noreturn;
+	bool land_noreturn_horizontal;
+	bool land_noreturn_vertical;
+	bool land_stayonground;
+	bool land_motor_lim;
+	bool land_onslope;
+
+	/* takeoff/launch states */
+	bool launch_detected;
+	bool usePreTakeoffThrust;
+
+	bool last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+
+	/* Landingslope object */
+	Landingslope landingslope;
+
+	float flare_curve_alt_rel_last;
 	/* heading hold */
 	float target_bearing;
 
+	/* Launch detection */
+	launchdetection::LaunchDetector launchDetector;
+
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
 	bool _airspeed_valid;				///< flag if a valid airspeed estimate exists
 	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
-	math::Dcm _R_nb;				///< current attitude
+	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
-	TECS						_tecs;
+	fwPosctrl::mTecs				_mTecs;
+	bool						_was_pos_control_mode;
 
 	struct {
 		float l1_period;
@@ -205,7 +238,16 @@ private:
 
 		float throttle_land_max;
 
-		float loiter_hold_radius;
+		float heightrate_p;
+		float speedrate_p;
+
+		float land_slope_angle;
+		float land_H1_virt;
+		float land_flare_alt_relative;
+		float land_thrust_lim_alt_relative;
+		float land_heading_hold_horizontal_distance;
+		float range_finder_rel_alt;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -239,7 +281,16 @@ private:
 
 		param_t throttle_land_max;
 
-		param_t loiter_hold_radius;
+		param_t heightrate_p;
+		param_t speedrate_p;
+
+		param_t land_slope_angle;
+		param_t land_H1_virt;
+		param_t land_flare_alt_relative;
+		param_t land_thrust_lim_alt_relative;
+		param_t land_heading_hold_horizontal_distance;
+		param_t range_finder_rel_alt;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -265,6 +316,12 @@ private:
 	bool		vehicle_airspeed_poll();
 
 	/**
+	 * Check for range finder updates.
+	 */
+	bool		range_finder_poll();
+
+
+	/**
 	 * Check for position updates.
 	 */
 	void		vehicle_attitude_poll();
@@ -272,7 +329,7 @@ private:
 	/**
 	 * Check for accel updates.
 	 */
-	void		vehicle_accel_poll();
+	void		vehicle_sensor_combined_poll();
 
 	/**
 	 * Check for set triplet updates.
@@ -280,13 +337,23 @@ private:
 	void		vehicle_setpoint_poll();
 
 	/**
+	 * Publish navigation capabilities
+	 */
+	void navigation_capabilities_publish();
+
+	/**
+	 * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
+	 */
+	float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
+
+	/**
 	 * Control position.
 	 */
-	bool		control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
-					 const struct vehicle_global_position_set_triplet_s &global_triplet);
+	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
+					 const struct position_setpoint_triplet_s &_pos_sp_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
-	void calculate_gndspeed_undershoot();
+	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -296,7 +363,30 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
+
+	/*
+	 * Reset takeoff state
+	 */
+	void reset_takeoff_state();
+
+	/*
+	 * Reset landing state
+	 */
+	void reset_landing_state();
+
+	/*
+	 * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
+	 * XXX need to clean up/remove this function once mtecs fully replaces TECS
+	 */
+	void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
+			float pitch_min_rad, float pitch_max_rad,
+			float throttle_min, float throttle_max, float throttle_cruise,
+			bool climbout_mode, float climbout_pitch_min_rad,
+			float altitude,
+			const math::Vector<3> &ground_speed,
+			tecs_mode mode = TECS_MODE_NORMAL);
+
 };
 
 namespace l1_control
@@ -308,43 +398,67 @@ namespace l1_control
 #endif
 static const int ERROR = -1;
 
-FixedwingPositionControl	*g_control;
+FixedwingPositionControl	*g_control = nullptr;
 }
 
 FixedwingPositionControl::FixedwingPositionControl() :
 
+	_mavlink_fd(-1),
 	_task_should_exit(false),
+	_task_running(false),
 	_control_task(-1),
 
 /* subscriptions */
 	_global_pos_sub(-1),
-	_global_set_triplet_sub(-1),
+	_pos_sp_triplet_sub(-1),
 	_att_sub(-1),
 	_airspeed_sub(-1),
 	_control_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
+	_range_finder_sub(-1),
 
 /* publications */
 	_attitude_sp_pub(-1),
 	_nav_capabilities_pub(-1),
 
+	_att(),
+	_att_sp(),
+	_nav_capabilities(),
+	_manual(),
+	_airspeed(),
+	_control_mode(),
+	_global_pos(),
+	_pos_sp_triplet(),
+	_sensor_combined(),
+	_range_finder(),
+
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+
 /* states */
-	_setpoint_valid(false),
 	_loiter_hold(false),
+	land_noreturn_horizontal(false),
+	land_noreturn_vertical(false),
+	land_stayonground(false),
+	land_motor_lim(false),
+	land_onslope(false),
+	launch_detected(false),
+	usePreTakeoffThrust(false),
+	last_manual(false),
+	flare_curve_alt_rel_last(0.0f),
+	launchDetector(),
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
-	land_noreturn(false)
+	_mTecs(),
+	_was_pos_control_mode(false)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
 	_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
-	_parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@@ -358,6 +472,13 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
+	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
+	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
+	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
+	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
+	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
+	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
+
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
 	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
@@ -370,6 +491,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.roll_throttle_compensation = 	param_find("FW_T_RLL2THR");
 	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
 	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
+	_parameter_handles.heightrate_p =			param_find("FW_T_HRATE_P");
+	_parameter_handles.speedrate_p =			param_find("FW_T_SRATE_P");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -407,7 +530,6 @@ FixedwingPositionControl::parameters_update()
 	/* L1 control parameters */
 	param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
 	param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
-	param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
@@ -435,25 +557,21 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
 	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
 
+	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
+
+	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
+	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
+	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
+	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
+	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
+
+	param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
+
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
 
-	_tecs.set_time_const(_parameters.time_const);
-	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
-	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
-	_tecs.set_throttle_damp(_parameters.throttle_damp);
-	_tecs.set_integrator_gain(_parameters.integrator_gain);
-	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
-	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
-	_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
-	_tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation));
-	_tecs.set_speed_weight(_parameters.speed_weight);
-	_tecs.set_pitch_damping(_parameters.pitch_damping);
-	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
-	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
-	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
-
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||
 	    _parameters.airspeed_max < 5.0f ||
@@ -464,6 +582,21 @@ FixedwingPositionControl::parameters_update()
 		return 1;
 	}
 
+	/* Update the landing slope */
+	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
+
+	/* Update and publish the navigation capabilities */
+	_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
+	_nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
+	_nav_capabilities.landing_flare_length = landingslope.flare_length();
+	navigation_capabilities_publish();
+
+	/* Update Launch Detector Parameters */
+	launchDetector.updateParams();
+
+	/* Update the mTecs */
+	_mTecs.updateParams();
+
 	return OK;
 }
 
@@ -482,8 +615,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 
 		if (!was_armed && _control_mode.flag_armed) {
-			_launch_lat = _global_pos.lat / 1e7f;
-			_launch_lon = _global_pos.lon / 1e7f;
+			_launch_lat = _global_pos.lat;
+			_launch_lon = _global_pos.lon;
 			_launch_alt = _global_pos.alt;
 			_launch_valid = true;
 		}
@@ -510,12 +643,23 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 		}
 	}
 
-	/* update TECS state */
-	_tecs.enable_airspeed(_airspeed_valid);
-
 	return airspeed_updated;
 }
 
+bool
+FixedwingPositionControl::range_finder_poll()
+{
+	/* check if there is a range finder measurement */
+	bool range_finder_updated;
+	orb_check(_range_finder_sub, &range_finder_updated);
+
+	if (range_finder_updated) {
+		orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
+	}
+
+	return range_finder_updated;
+}
+
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -533,14 +677,14 @@ FixedwingPositionControl::vehicle_attitude_poll()
 }
 
 void
-FixedwingPositionControl::vehicle_accel_poll()
+FixedwingPositionControl::vehicle_sensor_combined_poll()
 {
 	/* check if there is a new position */
-	bool accel_updated;
-	orb_check(_accel_sub, &accel_updated);
+	bool sensors_updated;
+	orb_check(_sensor_combined_sub, &sensors_updated);
 
-	if (accel_updated) {
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+	if (sensors_updated) {
+		orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 	}
 }
 
@@ -548,12 +692,11 @@ void
 FixedwingPositionControl::vehicle_setpoint_poll()
 {
 	/* check if there is a new setpoint */
-	bool global_sp_updated;
-	orb_check(_global_set_triplet_sub, &global_sp_updated);
+	bool pos_sp_triplet_updated;
+	orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
 
-	if (global_sp_updated) {
-		orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet);
-		_setpoint_valid = true;
+	if (pos_sp_triplet_updated) {
+		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
 	}
 }
 
@@ -595,17 +738,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 }
 
 void
-FixedwingPositionControl::calculate_gndspeed_undershoot()
+FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (_global_pos_valid) {
-		/* get ground speed vector */
-		math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy);
+	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
 
-		/* rotate with current attitude */
-		math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
+		/* rotate ground speed vector with current attitude */
+		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
 		yaw_vector.normalize();
-		float ground_speed_body = yaw_vector * ground_speed_vector;
+		float ground_speed_body = yaw_vector * ground_speed_2d;
+
+		/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
+		float distance = 0.0f;
+		float delta_altitude = 0.0f;
+		if (pos_sp_triplet.previous.valid) {
+			distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+			delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt;
+		} else {
+			distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+			delta_altitude = pos_sp_triplet.current.alt -  _global_pos.alt;
+		}
+
+		float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
+
 
 		/*
 		 * Ground speed undershoot is the amount of ground velocity not reached
@@ -616,32 +771,54 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
 		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
 		 * by wind). Not countering this would lead to a fly-away.
 		 */
-		_groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f);
+		_groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f);
 
 	} else {
 		_groundspeed_undershoot = 0;
 	}
 }
 
+void FixedwingPositionControl::navigation_capabilities_publish()
+{
+	if (_nav_capabilities_pub > 0) {
+		orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
+	} else {
+		_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
+	}
+}
+
+float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
+{
+	float rel_alt_estimated = current_alt - land_setpoint_alt;
+
+	/* only use range finder if:
+	 * parameter (range_finder_use_relative_alt) > 0
+	 * the measurement is valid
+	 * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
+	 */
+	if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
+		return rel_alt_estimated;
+	}
+
+	return range_finder.distance;
+
+}
+
 bool
-FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
-		const struct vehicle_global_position_set_triplet_s &global_triplet)
+FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
+		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 	bool setpoint = true;
 
-	calculate_gndspeed_undershoot();
+	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
+	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
-	// XXX re-visit
-	float baro_altitude = _global_pos.alt;
-
 	/* filter speed and altitude for controller */
-	math::Vector3 accel_body(_accel.x, _accel.y, _accel.z);
-	math::Vector3 accel_earth = _R_nb.transpose() * accel_body;
+	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 
-	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
-	float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
+	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
@@ -650,260 +827,311 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 	// 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();
+		if (!_was_pos_control_mode) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
 
-		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
-		_tecs.set_speed_weight(_parameters.speed_weight);
+		_was_pos_control_mode = true;
 
-		/* execute navigation once we have a setpoint */
-		if (_setpoint_valid) {
+		/* get circle mode */
+		bool was_circle_mode = _l1_control.circle_mode();
 
-			/* current waypoint (the one currently heading for) */
-			math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
+		/* current waypoint (the one currently heading for) */
+		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
-			/* previous waypoint */
-			math::Vector2f prev_wp;
+		/* current waypoint (the one currently heading for) */
+		math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
-			if (global_triplet.previous_valid) {
-				prev_wp.setX(global_triplet.previous.lat / 1e7f);
-				prev_wp.setY(global_triplet.previous.lon / 1e7f);
+		/* Initialize attitude controller integrator reset flags to 0 */
+		_att_sp.roll_reset_integral = false;
+		_att_sp.pitch_reset_integral = false;
+		_att_sp.yaw_reset_integral = false;
 
-			} else {
-				/*
-				 * No valid previous waypoint, go for the current wp.
-				 * This is automatically handled by the L1 library.
-				 */
-				prev_wp.setX(global_triplet.current.lat / 1e7f);
-				prev_wp.setY(global_triplet.current.lon / 1e7f);
+		/* previous waypoint */
+		math::Vector<2> prev_wp;
 
-			}
+		if (pos_sp_triplet.previous.valid) {
+			prev_wp(0) = (float)pos_sp_triplet.previous.lat;
+			prev_wp(1) = (float)pos_sp_triplet.previous.lon;
 
-			// XXX add RTL switch
-			if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
+		} else {
+			/*
+			 * No valid previous waypoint, go for the current wp.
+			 * This is automatically handled by the L1 library.
+			 */
+			prev_wp(0) = (float)pos_sp_triplet.current.lat;
+			prev_wp(1) = (float)pos_sp_triplet.current.lon;
 
-				math::Vector2f rtl_pos(_launch_lat, _launch_lon);
+		}
 
-				_l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+		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_2d);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
+						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
+						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
-				// XXX handle case when having arrived at home (loiter)
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
-				/* waypoint is a plain navigation waypoint */
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			/* waypoint is a loiter waypoint */
+			_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
+						  pos_sp_triplet.current.loiter_direction, ground_speed_2d);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
+						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
+						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 
-				/* waypoint is a loiter waypoint */
-				_l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius,
-							  global_triplet.current.loiter_direction, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+			/* Horizontal landing control */
+			/* switch to heading hold for the last meters, continue heading hold after */
+			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
+			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
+				/* heading hold, along the line connecting this and the last waypoint */
 
-				/* switch to heading hold for the last meters, continue heading hold after */
+				if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
+					if (pos_sp_triplet.previous.valid) {
+						target_bearing = bearing_lastwp_currwp;
+					} else {
+						target_bearing = _att.yaw;
+					}
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
+				}
 
-				float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
-				//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
-				if (wp_distance < 15.0f || land_noreturn) {
+//					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
 
-					/* heading hold, along the line connecting this and the last waypoint */
-					
+				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
 
-					// if (global_triplet.previous_valid) {
-					// 	target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
-					// } else {
+				/* limit roll motion to prevent wings from touching the ground first */
+				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
 
-					if (!land_noreturn)
-						target_bearing = _att.yaw;
-					//}
+				land_noreturn_horizontal = true;
 
-					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+			} else {
 
-					_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+				/* normal navigation */
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+			}
 
-					if (altitude_error > -5.0f)
-						land_noreturn = true;
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				} else {
 
-					/* normal navigation */
-					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				}
+			/* Vertical landing control */
+			//xxx: using the tecs altitude controller for slope control for now
 
-				/* do not go down too early */
-				if (wp_distance > 50.0f) {
-					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
-				}
+//				/* do not go down too early */
+//				if (wp_distance > 50.0f) {
+//					altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
+//				}
+			/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
+			// XXX this could make a great param
 
+			float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
+			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+			float airspeed_land = 1.3f * _parameters.airspeed_min;
+			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			/* Calculate altitude of last ordinary waypoint L */
+			float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
 
-				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
-				// XXX this could make a great param
+			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
 
-				float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
-				float land_pitch_min = math::radians(5.0f);
-				float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
-				float airspeed_land = _parameters.airspeed_min;
-				float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
+			float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
 
-				if (altitude_error > -4.0f) {
+			if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
-					/* land with minimal speed */
+				/* land with minimal speed */
 
-					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
-					_tecs.set_speed_weight(2.0f);
+//					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
+//					_tecs.set_speed_weight(2.0f);
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, flare_angle_rad,
-								    0.0f, _parameters.throttle_max, throttle_land,
-								    math::radians(-10.0f), math::radians(15.0f));
+				/* kill the throttle if param requests it */
+				throttle_max = _parameters.throttle_max;
 
-					/* kill the throttle if param requests it */
+				 if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
+					if (!land_motor_lim) {
+						land_motor_lim  = true;
+						mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
+					}
 
-					/* limit roll motion to prevent wings from touching the ground first */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
+				 }
 
-				} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
+				float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
 
-					/* minimize speed to approach speed */
+				/* avoid climbout */
+				if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground)
+				{
+					flare_curve_alt_rel = 0.0f; // stay on ground
+					land_stayonground = true;
+				}
+
+				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
+						calculate_target_airspeed(airspeed_land), eas2tas,
+						flare_pitch_angle_rad, math::radians(15.0f),
+						0.0f, throttle_max, throttle_land,
+						false, flare_pitch_angle_rad,
+						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
+						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
+
+				if (!land_noreturn_vertical) {
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
+					land_noreturn_vertical = true;
+				}
+				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, flare_angle_rad,
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				flare_curve_alt_rel_last = flare_curve_alt_rel;
+			} else {
 
+				 /* intersect glide slope:
+				  * minimize speed to approach speed
+				  * if current position is higher than the slope follow the glide slope (sink to the
+				  * glide slope)
+				  * also if the system captures the slope it should stay
+				  * on the slope (bool land_onslope)
+				  * if current position is below the slope continue at previous wp altitude
+				  * until the intersection with slope
+				  * */
+				float altitude_desired_rel = relative_alt;
+				if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
+					/* stay on slope */
+					altitude_desired_rel = landing_slope_alt_rel_desired;
+					if (!land_onslope) {
+						mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+						land_onslope = true;
+					}
 				} else {
+					/* continue horizontally */
+					altitude_desired_rel =  _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
+				}
 
-					/* normal cruise speed */
+				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
+						calculate_target_airspeed(airspeed_approach), eas2tas,
+						math::radians(_parameters.pitch_limit_min),
+						math::radians(_parameters.pitch_limit_max),
+						_parameters.throttle_min,
+						_parameters.throttle_max,
+						_parameters.throttle_cruise,
+						false,
+						math::radians(_parameters.pitch_limit_min),
+						_pos_sp_triplet.current.alt + relative_alt,
+						ground_speed);
+			}
+
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, math::radians(_parameters.pitch_limit_min),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+			/* Perform launch detection */
+			if(!launch_detected) { //do not do further checks once a launch was detected
+				if (launchDetector.launchDetectionEnabled()) {
+					static hrt_abstime last_sent = 0;
+					if(hrt_absolute_time() - last_sent > 4e6) {
+						mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
+						last_sent = hrt_absolute_time();
+					}
+
+					/* Tell the attitude controller to stop integrating while we are waiting
+					 * for the launch */
+					_att_sp.roll_reset_integral = true;
+					_att_sp.pitch_reset_integral = true;
+					_att_sp.yaw_reset_integral = true;
+
+					/* Detect launch */
+					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+					if (launchDetector.getLaunchDetected()) {
+						launch_detected = true;
+						mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
+					}
+				} else	{
+					/* no takeoff detection --> fly */
+					launch_detected = true;
+					warnx("launchdetection off");
 				}
+			}
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			if (launch_detected) {
+				usePreTakeoffThrust = false;
 
 				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
-				if (altitude_error > 10.0f) {
+				if (altitude_error > 15.0f) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+							calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+							eas2tas,
+							math::radians(_parameters.pitch_limit_min),
+							math::radians(_parameters.pitch_limit_max),
+							_parameters.throttle_min, _parameters.throttle_max,
+							_parameters.throttle_cruise,
+							true,
+							math::max(math::radians(pos_sp_triplet.current.pitch_min),
+							math::radians(10.0f)),
+							_global_pos.alt,
+							ground_speed,
+							TECS_MODE_TAKEOFF);
 
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
 
 				} else {
-
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, math::radians(_parameters.pitch_limit_min),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+							calculate_target_airspeed(_parameters.airspeed_trim),
+							eas2tas,
+								math::radians(_parameters.pitch_limit_min),
+								math::radians(_parameters.pitch_limit_max),
+								_parameters.throttle_min,
+								_parameters.throttle_max,
+								_parameters.throttle_cruise,
+								false,
+								math::radians(_parameters.pitch_limit_min),
+								_global_pos.alt,
+								ground_speed);
 				}
-			}
-
-			// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
-			//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
-			// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
-			//       (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid");
-
-			// XXX at this point we always want no loiter hold if a
-			// mission is active
-			_loiter_hold = false;
-
-		} else if (_control_mode.flag_armed) {
-
-			/* hold position, but only if armed, climb 20m in case this is engaged on ground level */
-
-			// XXX rework with smarter state machine
-
-			if (!_loiter_hold) {
-				_loiter_hold_lat = _global_pos.lat / 1e7f;
-				_loiter_hold_lon = _global_pos.lon / 1e7f;
-				_loiter_hold_alt = _global_pos.alt + 25.0f;
-				_loiter_hold = true;
-			}
-
-			altitude_error = _loiter_hold_alt - _global_pos.alt;
-
-			math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
-
-			/* loiter around current position */
-			_l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius,
-						  1, ground_speed);
-			_att_sp.roll_body = _l1_control.nav_roll();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
-
-			/* climb with full throttle if the altitude error is bigger than 5 meters */
-			bool climb_out = (altitude_error > 3);
-
-			float min_pitch;
-
-			if (climb_out) {
-				min_pitch = math::radians(20.0f);
 
 			} else {
-				min_pitch = math::radians(_parameters.pitch_limit_min);
+				usePreTakeoffThrust = true;
 			}
+		}
 
-			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim),
-						    _airspeed.indicated_airspeed_m_s, eas2tas,
-						    climb_out, min_pitch,
-						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+		// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
+		//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
+		// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
+		//       (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
 
-			if (climb_out) {
-				/* limit roll motion to ensure enough lift */
-				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
-			}
+		// XXX at this point we always want no loiter hold if a
+		// mission is active
+		_loiter_hold = false;
+
+		/* reset landing state */
+		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
+			reset_landing_state();
 		}
 
-		/* reset land state */
-		if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
-			land_noreturn = false;
+		/* reset takeoff/launch state */
+		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
+			reset_takeoff_state();
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -911,97 +1139,119 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			_att_sp.roll_reset_integral = true;
 		}
 
-	} else if (0/* easy mode enabled */) {
+	} else if (0/* posctrl mode enabled */) {
 
-		/** EASY FLIGHT **/
+		_was_pos_control_mode = false;
 
-		if (0/* switched from another mode to easy */) {
-			_seatbelt_hold_heading = _att.yaw;
-		}
+		/** POSCTRL FLIGHT **/
 
-		if (0/* easy on and manual control yaw non-zero */) {
-			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
-		}
+	if (0/* switched from another mode to posctrl */) {
+		_altctrl_hold_heading = _att.yaw;
+	}
 
-		/* climb out control */
-		bool climb_out = false;
+	if (0/* posctrl on and manual control yaw non-zero */) {
+		_altctrl_hold_heading = _att.yaw + _manual.r;
+	}
 
-		/* user wants to climb out */
-		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
-			climb_out = true;
-		}
+	//XXX not used
 
-		/* if in seatbelt mode, set airspeed based on manual control */
+	/* climb out control */
+//		bool climb_out = false;
+//
+//		/* user wants to climb out */
+//		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+//			climb_out = true;
+//		}
 
-		// XXX check if ground speed undershoot should be applied here
-		float seatbelt_airspeed = _parameters.airspeed_min +
-					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.throttle;
+	/* if in altctrl mode, set airspeed based on manual control */
 
-		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+	// XXX check if ground speed undershoot should be applied here
+	float altctrl_airspeed = _parameters.airspeed_min +
+					(_parameters.airspeed_max - _parameters.airspeed_min) *
+					_manual.z;
+
+		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
 		_att_sp.roll_body = _l1_control.nav_roll();
 		_att_sp.yaw_body = _l1_control.nav_bearing();
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
-					    seatbelt_airspeed,
-					    _airspeed.indicated_airspeed_m_s, eas2tas,
-					    false, _parameters.pitch_limit_min,
-					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
 
-	} else if (0/* seatbelt mode enabled */) {
+		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
+					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
+					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+					false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
+
+	} else if (0/* altctrl mode enabled */) {
+
+		_was_pos_control_mode = false;
 
-		/** SEATBELT FLIGHT **/
+		/** ALTCTRL FLIGHT **/
 
-		if (0/* switched from another mode to seatbelt */) {
-			_seatbelt_hold_heading = _att.yaw;
+		if (0/* switched from another mode to altctrl */) {
+			_altctrl_hold_heading = _att.yaw;
 		}
 
-		if (0/* seatbelt on and manual control yaw non-zero */) {
-			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		if (0/* altctrl on and manual control yaw non-zero */) {
+			_altctrl_hold_heading = _att.yaw + _manual.r;
 		}
 
-		/* if in seatbelt mode, set airspeed based on manual control */
+		/* if in altctrl mode, set airspeed based on manual control */
 
 		// XXX check if ground speed undershoot should be applied here
-		float seatbelt_airspeed = _parameters.airspeed_min +
+		float altctrl_airspeed = _parameters.airspeed_min +
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.throttle;
+					  _manual.z;
 
 		/* user switched off throttle */
-		if (_manual.throttle < 0.1f) {
+		if (_manual.z < 0.1f) {
 			throttle_max = 0.0f;
-			/* switch to pure pitch based altitude control, give up speed */
-			_tecs.set_speed_weight(0.0f);
 		}
 
 		/* climb out control */
 		bool climb_out = false;
 
 		/* user wants to climb out */
-		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+		if (_manual.x > 0.3f && _manual.z > 0.8f) {
 			climb_out = true;
 		}
 
-		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
-		_att_sp.roll_body =	_manual.roll;
-		_att_sp.yaw_body =	_manual.yaw;
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
-					    seatbelt_airspeed,
-					    _airspeed.indicated_airspeed_m_s, eas2tas,
-					    climb_out, _parameters.pitch_limit_min,
-					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
+		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
+		_att_sp.roll_body =	_manual.y;
+		_att_sp.yaw_body =	_manual.r;
+		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
+					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
+					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+					climb_out, math::radians(_parameters.pitch_limit_min),
+					_global_pos.alt, ground_speed);
 
 	} else {
 
+		_was_pos_control_mode = false;
+
 		/** MANUAL FLIGHT **/
 
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
+
+		/* reset landing and takeoff state */
+		if (!last_manual) {
+			reset_landing_state();
+			reset_takeoff_state();
+		}
+	}
+
+	if (usePreTakeoffThrust) {
+		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+	}
+	else {
+		_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
+	}
+	_att_sp.pitch_body = _mTecs.getPitchSetpoint();
+
+	if (_control_mode.flag_control_position_enabled) {
+		last_manual = false;
+	} else {
+		last_manual = true;
 	}
 
-	_att_sp.pitch_body = _tecs.get_pitch_demand();
-	_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
 
 	return setpoint;
 }
@@ -1018,13 +1268,14 @@ FixedwingPositionControl::task_main()
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet));
+	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_control_mode_sub, 200);
@@ -1047,6 +1298,8 @@ FixedwingPositionControl::task_main()
 	fds[1].fd = _global_pos_sub;
 	fds[1].events = POLLIN;
 
+	_task_running = true;
+
 	while (!_task_should_exit) {
 
 		/* wait for up to 500ms for data */
@@ -1080,6 +1333,11 @@ FixedwingPositionControl::task_main()
 		/* only run controller if position changed */
 		if (fds[1].revents & POLLIN) {
 
+			/* XXX Hack to get mavlink output going */
+			if (_mavlink_fd < 0) {
+				/* try to open the mavlink log device every once in a while */
+				_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+			}
 
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -1097,18 +1355,19 @@ FixedwingPositionControl::task_main()
 
 			vehicle_attitude_poll();
 			vehicle_setpoint_poll();
-			vehicle_accel_poll();
+			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
+			range_finder_poll();
 			// vehicle_baro_poll();
 
-			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
-			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
+			math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
 
 			/*
 			 * Attempt to control position, on success (= sensors present and not in manual mode),
 			 * publish setpoint.
 			 */
-			if (control_position(current_position, ground_speed, _global_triplet)) {
+			if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
 				_att_sp.timestamp = hrt_absolute_time();
 
 				/* lazily publish the setpoint only once available */
@@ -1121,19 +1380,17 @@ FixedwingPositionControl::task_main()
 					_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 				}
 
-				float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy);
+				/* XXX check if radius makes sense here */
+				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
-				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
+				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
 
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;
 
-					if (_nav_capabilities_pub > 0) {
-						orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
-					} else {
-						_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
-					}
+					navigation_capabilities_publish();
+
 				}
 
 			}
@@ -1143,12 +1400,54 @@ FixedwingPositionControl::task_main()
 		perf_end(_loop_perf);
 	}
 
+	_task_running = false;
+
 	warnx("exiting.\n");
 
 	_control_task = -1;
 	_exit(0);
 }
 
+void FixedwingPositionControl::reset_takeoff_state()
+{
+	launch_detected = false;
+	usePreTakeoffThrust = false;
+	launchDetector.reset();
+}
+
+void FixedwingPositionControl::reset_landing_state()
+{
+	land_noreturn_horizontal = false;
+	land_noreturn_vertical = false;
+	land_stayonground = false;
+	land_motor_lim = false;
+	land_onslope = false;
+}
+
+void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
+		float pitch_min_rad, float pitch_max_rad,
+		float throttle_min, float throttle_max, float throttle_cruise,
+		bool climbout_mode, float climbout_pitch_min_rad,
+		float altitude,
+		const math::Vector<3> &ground_speed,
+		tecs_mode mode)
+{
+	/* Using mtecs library: prepare arguments for mtecs call */
+	float flightPathAngle = 0.0f;
+	float ground_speed_length = ground_speed.length();
+	if (ground_speed_length > FLT_EPSILON) {
+		flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
+	}
+	fwPosctrl::LimitOverride limitOverride;
+	if (climbout_mode) {
+		limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+	} else {
+		limitOverride.disablePitchMinOverride();
+	}
+	_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+			limitOverride);
+}
+
 int
 FixedwingPositionControl::start()
 {
@@ -1158,7 +1457,7 @@ FixedwingPositionControl::start()
 	_control_task = task_spawn_cmd("fw_pos_control_l1",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       4048,
+				       2000,
 				       (main_t)&FixedwingPositionControl::task_main_trampoline,
 				       nullptr);
 
@@ -1191,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[])
 			err(1, "start failed");
 		}
 
+		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
+		while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) {
+			usleep(50000);
+			printf(".");
+			fflush(stdout);
+		}
+		printf("\n");
+
 		exit(0);
 	}
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 31a9cdefa..52128e1b7 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -40,12 +40,10 @@
  */
 
 #include <nuttx/config.h>
-
 #include <systemlib/param/param.h>
 
 /*
  * Controller parameters, accessible via MAVLink
- *
  */
 
 /**
@@ -74,17 +72,6 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
 PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
 
 /**
- * Default Loiter Radius
- *
- * This radius is used when no other loiter radius is set.
- *
- * @min 10.0
- * @max 100.0
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
-
-/**
  * Cruise throttle
  *
  * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
@@ -119,48 +106,272 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
  */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
 
-
+/**
+ * Controller roll limit
+ *
+ * The maximum roll the controller will output.
+ *
+ * @unit degrees
+ * @min 0.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
 
+/**
+ * Throttle limit max
+ *
+ * This is the maximum throttle % that can be used by the controller. 
+ * For overpowered aircraft, this should be reduced to a value that 
+ * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 
+/**
+ * Throttle limit min
+ *
+ * This is the minimum throttle % that can be used by the controller. 
+ * For electric aircraft this will normally be set to zero, but can be set 
+ * to a small non-zero value if a folding prop is fitted to prevent the 
+ * prop from folding and unfolding repeatedly in-flight or to provide 
+ * some aerodynamic drag from a turning prop to improve the descent rate.
+ *
+ * For aircraft with internal combustion engine this parameter should be set
+ * for desired idle rpm.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 
-
-PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
-
+/**
+ * Throttle limit value before flare
+ *
+ * This throttle value will be set as throttle limit at FW_LND_TLALT, 
+ * before arcraft will flare.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 
+/**
+ * Maximum climb rate
+ *
+ * This is the best climb rate that the aircraft can achieve with 
+ * the throttle set to THR_MAX and the airspeed set to the 
+ * default value. For electric aircraft make sure this number can be 
+ * achieved towards the end of flight when the battery voltage has reduced. 
+ * The setting of this parameter can be checked by commanding a positive 
+ * altitude change of 100m in loiter, RTL or guided mode. If the throttle 
+ * required to climb is close to THR_MAX and the aircraft is maintaining 
+ * airspeed, then this parameter is set correctly. If the airspeed starts 
+ * to reduce, then the parameter is set to high, and if the throttle 
+ * demand required to climb and maintain speed is noticeably less than 
+ * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or 
+ * FW_THR_MAX reduced.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 
-
+/**
+ * Minimum descent rate
+ *
+ * This is the sink rate of the aircraft with the throttle 
+ * set to THR_MIN and flown at the same airspeed as used 
+ * to measure FW_T_CLMB_MAX.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
 
+/**
+ * Maximum descent rate
+ *
+ * This sets the maximum descent rate that the controller will use. 
+ * If this value is too large, the aircraft can over-speed on descent. 
+ * This should be set to a value that can be achieved without 
+ * exceeding the lower pitch angle limit and without over-speeding 
+ * the aircraft.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 
+/**
+ * TECS time constant
+ *
+ * This is the time constant of the TECS control algorithm (in seconds). 
+ * Smaller values make it faster to respond, larger values make it slower
+ * to respond.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
 
-
+/**
+ * Throttle damping factor
+ *
+ * This is the damping gain for the throttle demand loop. 
+ * Increase to add damping to correct for oscillations in speed and height.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
 
-
+/**
+ * Integrator gain
+ *
+ * This is the integrator gain on the control loop. 
+ * Increasing this gain increases the speed at which speed 
+ * and height offsets are trimmed out, but reduces damping and 
+ * increases overshoot.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
 
-
+/**
+ * Maximum vertical acceleration
+ *
+ * This is the maximum vertical acceleration (in metres/second square)
+ * either up or down that the controller will use to correct speed 
+ * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
+ * allows for reasonably aggressive pitch changes if required to recover 
+ * from under-speed conditions.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
 
-
+/**
+ * Complementary filter "omega" parameter for height
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary 
+ * filter used to fuse vertical acceleration and barometric height to obtain 
+ * an estimate of height rate and height. Increasing this frequency weights 
+ * the solution more towards use of the barometer, whilst reducing it weights 
+ * the solution more towards use of the accelerometer data.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
 
-
+/**
+ * Complementary filter "omega" parameter for speed
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary 
+ * filter used to fuse longitudinal acceleration and airspeed to obtain an 
+ * improved airspeed estimate. Increasing this frequency weights the solution
+ * more towards use of the arispeed sensor, whilst reducing it weights the 
+ * solution more towards use of the accelerometer data.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
 
-
+/**
+ * Roll -> Throttle feedforward
+ *
+ * Increasing this gain turn increases the amount of throttle that will 
+ * be used to compensate for the additional drag created by turning. 
+ * Ideally this should be set to  approximately 10 x the extra sink rate 
+ * in m/s created by a 45 degree bank turn. Increase this gain if 
+ * the aircraft initially loses energy in turns and reduce if the 
+ * aircraft initially gains energy in turns. Efficient high aspect-ratio 
+ * aircraft (eg powered sailplanes) can use a lower value, whereas 
+ * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
 
-
+/**
+ * Speed <--> Altitude priority
+ *
+ * This parameter adjusts the amount of weighting that the pitch control 
+ * applies to speed vs height errors. Setting it to 0.0 will cause the 
+ * pitch control to control height and ignore speed errors. This will 
+ * normally improve height accuracy but give larger airspeed errors. 
+ * Setting it to 2.0 will cause the pitch control loop to control speed 
+ * and ignore height errors. This will normally reduce airspeed errors, 
+ * but give larger height errors. The default value of 1.0 allows the pitch 
+ * control to simultaneously control height and speed. 
+ * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
+ * adjust its pitch angle to maintain airspeed, ignoring changes in height).
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
 
-
+/**
+ * Pitch damping factor
+ *
+ * This is the damping gain for the pitch demand loop. Increase to add 
+ * damping to correct for oscillations in height. The default value of 0.0 
+ * will work well provided the pitch to servo controller has been tuned 
+ * properly.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 
+/**
+ * Height rate P factor
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
+
+/**
+ * Speed rate P factor
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
 
-PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
+/**
+ * Landing slope angle
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
+
+/**
+ *
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
+
+/**
+ * Landing flare altitude (relative)
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
+
+/**
+ * Landing throttle limit altitude (relative)
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
+
+/**
+ * Landing heading hold horizontal distance
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
+
+/**
+ * Relative altitude threshold for range finder measurements for use during landing
+ *
+ * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
+ * set to < 0 to disable
+ * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp
new file mode 100644
index 000000000..42e00da05
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/landingslope.cpp
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *
+ * 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: landingslope.cpp
+ *
+ */
+
+#include "landingslope.h"
+
+#include <nuttx/config.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <mathlib/mathlib.h>
+
+void Landingslope::update(float landing_slope_angle_rad_new,
+		float flare_relative_alt_new,
+		float motor_lim_relative_alt_new,
+		float H1_virt_new)
+{
+
+	_landing_slope_angle_rad = landing_slope_angle_rad_new;
+	_flare_relative_alt = flare_relative_alt_new;
+	_motor_lim_relative_alt = motor_lim_relative_alt_new;
+	_H1_virt = H1_virt_new;
+
+	calculateSlopeValues();
+}
+
+void Landingslope::calculateSlopeValues()
+{
+	_H0 =  _flare_relative_alt + _H1_virt;
+	_d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad);
+	_flare_constant = (_H0 * _d1)/_flare_relative_alt;
+	_flare_length = - logf(_H1_virt/_H0) * _flare_constant;
+	_horizontal_slope_displacement = (_flare_length - _d1);
+}
+
+float  Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
+{
+	return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad);
+}
+
+float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
+{
+	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return getLandingSlopeRelativeAltitude(wp_landing_distance);
+	else
+		return 0.0f;
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude)
+{
+	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
+{
+	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude);
+	else
+		return wp_altitude;
+}
+
+float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
+{
+	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+	else
+		return 0.0f;
+}
+
+float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+{
+
+	return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
+}
+
diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h
new file mode 100644
index 000000000..a5975ad43
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/landingslope.h
@@ -0,0 +1,145 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *
+ * 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: landingslope.h
+ *
+ */
+
+#ifndef LANDINGSLOPE_H_
+#define LANDINGSLOPE_H_
+
+#include <math.h>
+#include <systemlib/err.h>
+
+class Landingslope
+{
+private:
+	/* see Documentation/fw_landing.png for a plot of the landing slope */
+	float _landing_slope_angle_rad;			/**< phi in the plot */
+	float _flare_relative_alt;				/**< h_flare,rel in the plot */
+	float _motor_lim_relative_alt;
+	float _H1_virt;							/**< H1 in the plot */
+	float _H0;								/**< h_flare,rel + H1 in the plot */
+	float _d1;								/**< d1 in the plot */
+	float _flare_constant;
+	float _flare_length;					/**< d1 + delta d in the plot */
+	float _horizontal_slope_displacement;  /**< delta d in the plot */
+
+	void calculateSlopeValues();
+
+public:
+	Landingslope() {}
+	~Landingslope() {}
+
+
+	/**
+	 *
+	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	float getLandingSlopeRelativeAltitude(float wp_landing_distance);
+
+	/**
+	 *
+	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 * Performs check if aircraft is in front of waypoint to avoid climbout
+	 */
+	float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
+
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude);
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 * Performs check if aircraft is in front of waypoint to avoid climbout
+	 */
+	float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
+
+	/**
+	 *
+	 * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	__EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative
+	}
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude;
+	}
+
+	/**
+	 *
+	 * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
+	 */
+	__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
+
+	}
+
+	float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
+
+	float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
+
+	void update(float landing_slope_angle_rad_new,
+			float flare_relative_alt_new,
+			float motor_lim_relative_alt_new,
+			float H1_virt_new);
+
+
+	inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
+	inline float flare_relative_alt() {return _flare_relative_alt;}
+	inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;}
+	inline float H1_virt() {return _H1_virt;}
+	inline float H0() {return _H0;}
+	inline float d1() {return _d1;}
+	inline float flare_constant() {return _flare_constant;}
+	inline float flare_length() {return _flare_length;}
+	inline float horizontal_slope_displacement() {return _horizontal_slope_displacement;}
+
+};
+
+
+#endif /* LANDINGSLOPE_H_ */
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index b00b9aa5a..15b575b50 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/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
@@ -38,4 +38,10 @@
 MODULE_COMMAND	= fw_pos_control_l1
 
 SRCS		= fw_pos_control_l1_main.cpp \
-		  fw_pos_control_l1_params.c
+		  fw_pos_control_l1_params.c \
+		  landingslope.cpp \
+		  mtecs/mTecs.cpp \
+		  mtecs/limitoverride.cpp \
+		  mtecs/mTecs_params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
index ecd62e81c..58795edb6 100644
--- a/src/lib/mathlib/math/Vector2f.hpp
+++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,49 +32,40 @@
  *
  ****************************************************************************/
 
+
 /**
- * @file Vector2f.hpp
+ * @file limitoverride.cpp
  *
- * math 3 vector
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#pragma once
+#include "limitoverride.h"
 
-#include "Vector.hpp"
+namespace fwPosctrl {
 
-namespace math
+bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+		BlockOutputLimiter &outputLimiterPitch)
 {
+	bool ret = false;
 
-class __EXPORT Vector2f :
-	public Vector
-{
-public:
-	Vector2f();
-	Vector2f(const Vector &right);
-	Vector2f(float x, float y);
-	Vector2f(const float *data);
-	virtual ~Vector2f();
-	float cross(const Vector2f &b) const;
-	float operator %(const Vector2f &v) const;
-    float operator *(const Vector2f &v) const;
-    inline Vector2f operator*(const float &right) const {
-		return Vector::operator*(right);
+	if (overrideThrottleMinEnabled)	{
+		outputLimiterThrottle.setMin(overrideThrottleMin);
+		ret = true;
+	}
+	if (overrideThrottleMaxEnabled)	{
+		outputLimiterThrottle.setMax(overrideThrottleMax);
+		ret = true;
+	}
+	if (overridePitchMinEnabled)	{
+		outputLimiterPitch.setMin(overridePitchMin);
+		ret = true;
+	}
+	if (overridePitchMaxEnabled)	{
+		outputLimiterPitch.setMax(overridePitchMax);
+		ret = true;
 	}
 
-	/**
-	 * accessors
-	 */
-	void setX(float x) { (*this)(0) = x; }
-	void setY(float y) { (*this)(1) = y; }
-	const float &getX() const { return (*this)(0); }
-	const float &getY() const { return (*this)(1); }
-};
-    
-class __EXPORT Vector2 :
-	public Vector2f
-{
-};
-
-int __EXPORT vector2fTest();
-} // math
+	return ret;
+}
 
+} /* namespace fwPosctrl */
diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
new file mode 100644
index 000000000..64c2e7bbd
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 limitoverride.h
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+#ifndef LIMITOVERRIDE_H_
+#define LIMITOVERRIDE_H_
+
+#include "mTecs_blocks.h"
+
+namespace fwPosctrl
+{
+
+/* A small class which provides helper functions to override control output limits which are usually set by
+* parameters in special cases
+*/
+class LimitOverride
+{
+public:
+	LimitOverride() :
+		overrideThrottleMinEnabled(false),
+		overrideThrottleMaxEnabled(false),
+		overridePitchMinEnabled(false),
+		overridePitchMaxEnabled(false)
+	{};
+
+	~LimitOverride() {};
+
+	/*
+	* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
+	* this class (if enabled)
+	* @return true if the limit was applied
+	*/
+	bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+			BlockOutputLimiter &outputLimiterPitch);
+
+	/* Functions to enable or disable the override */
+	void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
+			&overrideThrottleMin, value); }
+	void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
+	void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
+			&overrideThrottleMax, value); }
+	void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
+	void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
+			&overridePitchMin, value); }
+	void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
+	void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
+			&overridePitchMax, value); }
+	void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
+
+protected:
+	bool overrideThrottleMinEnabled;
+	float overrideThrottleMin;
+	bool overrideThrottleMaxEnabled;
+	float overrideThrottleMax;
+	bool overridePitchMinEnabled;
+	float overridePitchMin; //in degrees (replaces param values)
+	bool overridePitchMaxEnabled;
+	float overridePitchMax; //in degrees (replaces param values)
+
+	/* Enable a specific limit override */
+	void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; };
+
+	/* Disable a specific limit override */
+	void disable(bool *flag) { *flag = false; };
+};
+
+} /* namespace fwPosctrl */
+
+#endif /* LIMITOVERRIDE_H_ */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
new file mode 100644
index 000000000..2e37d166e
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -0,0 +1,313 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 mTecs.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mTecs.h"
+
+#include <lib/geo/geo.h>
+#include <stdio.h>
+
+namespace fwPosctrl {
+
+mTecs::mTecs() :
+	SuperBlock(NULL, "MT"),
+	/* Parameters */
+	_mTecsEnabled(this, "ENABLED"),
+	_airspeedMin(this, "FW_AIRSPD_MIN", false),
+	/* Publications */
+	_status(&getPublications(), ORB_ID(tecs_status)),
+	/* control blocks */
+	_controlTotalEnergy(this, "THR"),
+	_controlEnergyDistribution(this, "PIT", true),
+	_controlAltitude(this, "FPA", true),
+	_controlAirSpeed(this, "ACC"),
+	_flightPathAngleLowpass(this, "FPA_LP"),
+	_airspeedLowpass(this, "A_LP"),
+	_airspeedDerivative(this, "AD"),
+	_throttleSp(0.0f),
+	_pitchSp(0.0f),
+	_BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
+	_BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
+	_BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"),
+	_BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true),
+	_BlockOutputLimiterLandThrottle(this, "LND_THR"),
+	_BlockOutputLimiterLandPitch(this, "LND_PIT", true),
+	timestampLastIteration(hrt_absolute_time()),
+	_firstIterationAfterReset(true),
+	_dtCalculated(false),
+	_counter(0),
+	_debug(false)
+{
+}
+
+mTecs::~mTecs()
+{
+}
+
+int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
+{
+	/* check if all input arguments are numbers and abort if not so */
+	if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
+			!isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
+		return -1;
+	}
+
+	/* time measurement */
+	updateTimeMeasurement();
+
+	/* calculate flight path angle setpoint from altitude setpoint */
+	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
+
+	/* Debug output */
+	if (_counter % 10 == 0) {
+		debug("***");
+		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
+	}
+
+	/* Write part of the status message */
+	_status.altitudeSp = altitudeSp;
+	_status.altitude = altitude;
+
+
+	/* use flightpath angle setpoint for total energy control */
+	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
+			airspeedSp, mode, limitOverride);
+}
+
+int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
+{
+	/* check if all input arguments are numbers and abort if not so */
+	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
+			!isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
+		return -1;
+	}
+
+	/* time measurement */
+	updateTimeMeasurement();
+
+	/* Filter airspeed */
+	float airspeedFiltered = _airspeedLowpass.update(airspeed);
+
+	/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
+	float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
+
+	/* Debug output */
+	if (_counter % 10 == 0) {
+		debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
+				"accelerationLongitudinalSp%.4f",
+				(double)airspeedSp, (double)airspeed,
+				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
+	}
+
+	/* Write part of the status message */
+	_status.airspeedSp = airspeedSp;
+	_status.airspeed = airspeed;
+	_status.airspeedFiltered = airspeedFiltered;
+
+
+	/* use longitudinal acceleration setpoint for total energy control */
+	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
+			accelerationLongitudinalSp, mode, limitOverride);
+}
+
+int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
+		float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
+{
+	/* check if all input arguments are numbers and abort if not so */
+	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
+			!isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
+		return -1;
+	}
+	/* time measurement */
+	updateTimeMeasurement();
+
+	/* update parameters first */
+	updateParams();
+
+	/* Filter flightpathangle */
+	float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
+
+	/* calculate values (energies) */
+	float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
+	float airspeedDerivative = 0.0f;
+	if(_airspeedDerivative.getDt() > 0.0f) {
+		airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
+	}
+	float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
+	float airspeedDerivativeSp = accelerationLongitudinalSp;
+	float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
+	float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
+
+	float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
+	float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
+	float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
+	float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
+
+	float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
+	float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
+	float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
+	float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
+
+	/* Debug output */
+	if (_counter % 10 == 0) {
+		debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f",
+				(double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm);
+		debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f",
+				(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
+	}
+
+	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
+	if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
+		mode = TECS_MODE_UNDERSPEED;
+	}
+
+	/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
+	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
+	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
+	if (mode == TECS_MODE_TAKEOFF) {
+		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
+		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
+	} else if (mode == TECS_MODE_LAND) {
+		// only limit pitch but do not limit throttle
+		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
+	} else if (mode == TECS_MODE_LAND_THROTTLELIM) {
+		outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
+		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
+	} else if (mode == TECS_MODE_UNDERSPEED) {
+		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
+		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
+	}
+
+	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
+	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
+	 * is running) */
+	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
+
+	/* Write part of the status message */
+	_status.flightPathAngleSp = flightPathAngleSp;
+	_status.flightPathAngle = flightPathAngle;
+	_status.flightPathAngleFiltered = flightPathAngleFiltered;
+	_status.airspeedDerivativeSp = airspeedDerivativeSp;
+	_status.airspeedDerivative = airspeedDerivative;
+	_status.totalEnergyRateSp = totalEnergyRateSp;
+	_status.totalEnergyRate = totalEnergyRate;
+	_status.energyDistributionRateSp = energyDistributionRateSp;
+	_status.energyDistributionRate = energyDistributionRate;
+	_status.mode = mode;
+
+	/** update control blocks **/
+	/* update total energy rate control block */
+	_throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle);
+
+	/* update energy distribution rate control block */
+	_pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch);
+
+
+	if (_counter % 10 == 0) {
+		debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f",
+				(double)_throttleSp, (double)_pitchSp,
+				(double)flightPathAngleSp, (double)flightPathAngle,
+				(double)accelerationLongitudinalSp, (double)airspeedDerivative);
+	}
+
+	/* publish status messge */
+	_status.update();
+
+	/* clean up */
+	_firstIterationAfterReset = false;
+	_dtCalculated = false;
+
+	_counter++;
+
+	return 0;
+}
+
+void mTecs::resetIntegrators()
+{
+	_controlTotalEnergy.getIntegral().setY(0.0f);
+	_controlEnergyDistribution.getIntegral().setY(0.0f);
+	timestampLastIteration = hrt_absolute_time();
+	_firstIterationAfterReset = true;
+}
+
+void mTecs::resetDerivatives(float airspeed)
+{
+	_airspeedDerivative.setU(airspeed);
+}
+
+
+void mTecs::updateTimeMeasurement()
+{
+	if (!_dtCalculated) {
+		float deltaTSeconds = 0.0f;
+		if (!_firstIterationAfterReset) {
+			hrt_abstime timestampNow = hrt_absolute_time();
+			deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
+			timestampLastIteration = timestampNow;
+		}
+		setDt(deltaTSeconds);
+
+		_dtCalculated = true;
+	}
+}
+
+void mTecs::debug_print(const char *fmt, va_list args)
+{
+	fprintf(stderr, "%s: ", "[mtecs]");
+	vfprintf(stderr, fmt, args);
+
+	fprintf(stderr, "\n");
+}
+
+void mTecs::debug(const char *fmt, ...) {
+
+	if (!_debug) {
+		return;
+	}
+
+	va_list args;
+
+	va_start(args, fmt);
+	debug_print(fmt, args);
+}
+
+} /* namespace fwPosctrl */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
new file mode 100644
index 000000000..efa89a5d3
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 mTecs.h
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+#ifndef MTECS_H_
+#define MTECS_H_
+
+#include "mTecs_blocks.h"
+#include "limitoverride.h"
+
+#include <controllib/block/BlockParam.hpp>
+#include <drivers/drv_hrt.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/tecs_status.h>
+
+namespace fwPosctrl
+{
+
+/* Main class of the mTecs */
+class mTecs : public control::SuperBlock
+{
+public:
+	mTecs();
+	virtual ~mTecs();
+
+	/*
+	 * Control in altitude setpoint and speed mode
+	 */
+	int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
+
+	/*
+	 * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
+	 */
+	int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
+
+	/*
+	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
+	 */
+	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
+			float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
+
+	/*
+	 * Reset all integrators
+	 */
+	void resetIntegrators();
+
+	/*
+	 * Reset all derivative calculations
+	 */
+	void resetDerivatives(float airspeed);
+
+	/* Accessors */
+	bool getEnabled() { return _mTecsEnabled.get() > 0; }
+	float getThrottleSetpoint() { return _throttleSp; }
+	float getPitchSetpoint() { return _pitchSp; }
+	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
+
+protected:
+	/* parameters */
+	control::BlockParamInt _mTecsEnabled;		/**< 1 if mTecs is enabled */
+	control::BlockParamFloat _airspeedMin;		/**< minimal airspeed */
+
+	/* Publications */
+	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
+
+	/* control blocks */
+	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
+								  is throttle */
+	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control:
+								  output is pitch */
+	BlockPDLimited	_controlAltitude;			/**< PD controller for altitude: output is the flight
+								  path angle setpoint */
+	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration
+								  setpoint */
+
+	/* Other calculation Blocks */
+	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
+	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
+	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
+
+	/* Output setpoints */
+	float _throttleSp;				/**< Throttle Setpoint from 0 to 1 */
+	float _pitchSp;					/**< Pitch Setpoint from -pi to pi */
+
+	/* Output Limits in special modes */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;	    /**< Throttle Limits during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;	    /**< Pitch Limit during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;   /**< Throttle Limits when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;	    /**< Pitch Limit when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterLandThrottle;	    /**< Throttle Limits during landing (only in
+								      last phase)*/
+	BlockOutputLimiter _BlockOutputLimiterLandPitch;	    /**< Pitch Limit during landing */
+
+	/* Time measurements */
+	hrt_abstime timestampLastIteration;	/**< Saves the result of hrt_absolute_time() of the last iteration */
+
+	bool _firstIterationAfterReset;		/**< True during the first iteration after a reset */
+	bool _dtCalculated;			/**< True if dt has been calculated in this iteration */
+
+	int _counter;
+	bool _debug;				///< Set true to enable debug output
+
+
+	static void	debug_print(const char *fmt, va_list args);
+	void		debug(const char *fmt, ...);
+
+	/*
+	 * Measure and update the time step dt if this was not already done in the current iteration
+	 */
+	void updateTimeMeasurement();
+};
+
+} /* namespace fwPosctrl */
+
+#endif /* MTECS_H_ */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
new file mode 100644
index 000000000..c22e60ae0
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 mTecs_blocks.h
+ *
+ * Custom blocks for the mTecs
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#pragma once
+
+#include <controllib/blocks.hpp>
+#include <systemlib/err.h>
+
+namespace fwPosctrl
+{
+
+using namespace control;
+
+/* An block which can be used to limit the output */
+class BlockOutputLimiter: public SuperBlock
+{
+public:
+// methods
+	BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) :
+		SuperBlock(parent, name),
+		_isAngularLimit(fAngularLimit),
+		_min(this, "MIN"),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockOutputLimiter() {};
+	/*
+	 * Imposes the limits given by _min and _max on value
+	 *
+	 * @param value is changed to be on the interval _min to _max
+	 * @param difference if the value is changed this corresponds to the change of value * (-1)
+     * otherwise unchanged
+	 * @return: true if the limit is applied, false otherwise
+	 */
+	bool limit(float& value, float& difference) {
+		float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
+		float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
+		if (value < minimum) {
+			difference = value - minimum;
+			value = minimum;
+			return true;
+		} else if (value > maximum) {
+			difference = value - maximum;
+			value = maximum;
+			return true;
+		}
+		return false;
+	}
+//accessor:
+	bool isAngularLimit() {return _isAngularLimit ;}
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+	void setMin(float value) { _min.set(value); }
+	void setMax(float value) { _max.set(value); }
+protected:
+//attributes
+	bool _isAngularLimit;
+	control::BlockParamFloat _min;
+	control::BlockParamFloat _max;
+};
+
+
+/* A combination of feed forward, P and I gain using the output limiter*/
+class BlockFFPILimited: public SuperBlock
+{
+public:
+// methods
+	BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_outputLimiter(this, "", isAngularLimit),
+		_integral(this, "I"),
+		_kFF(this, "FF"),
+		_kP(this, "P"),
+		_kI(this, "I"),
+		_offset(this, "OFF")
+	{};
+	virtual ~BlockFFPILimited() {};
+	float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
+// accessors
+	BlockIntegral &getIntegral() { return _integral; }
+	float getKFF() { return _kFF.get(); }
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	float getOffset() { return _offset.get(); }
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+protected:
+	BlockOutputLimiter _outputLimiter;
+
+	float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);}
+	float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) {
+		float difference = 0.0f;
+		float integralYPrevious = _integral.getY();
+		float output = calcUnlimitedOutput(inputValue, inputError);
+		if(outputLimiter.limit(output, difference) &&
+			(((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
+			((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
+				getIntegral().setY(integralYPrevious);
+		}
+		return output;
+	}
+private:
+	BlockIntegral _integral;
+	BlockParamFloat _kFF;
+	BlockParamFloat _kP;
+	BlockParamFloat _kI;
+	BlockParamFloat _offset;
+};
+
+/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/
+class BlockFFPILimitedCustom: public BlockFFPILimited
+{
+public:
+// methods
+	BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		BlockFFPILimited(parent, name, isAngularLimit)
+		{};
+	virtual ~BlockFFPILimitedCustom() {};
+	float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) {
+		return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter);
+	}
+};
+
+/* A combination of P gain and output limiter */
+class BlockPLimited: public SuperBlock
+{
+public:
+// methods
+	BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_kP(this, "P"),
+		_outputLimiter(this, "", isAngularLimit)
+	{};
+	virtual ~BlockPLimited() {};
+	float update(float input) {
+		float difference = 0.0f;
+		float output = getKP() * input;
+		getOutputLimiter().limit(output, difference);
+		return output;
+	}
+// accessors
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+	float getKP() { return _kP.get(); }
+private:
+	control::BlockParamFloat _kP;
+	BlockOutputLimiter _outputLimiter;
+};
+
+/* A combination of P, D gains and output limiter */
+class BlockPDLimited: public SuperBlock
+{
+public:
+// methods
+	BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_kP(this, "P"),
+		_kD(this, "D"),
+		_derivative(this, "D"),
+		_outputLimiter(this, "", isAngularLimit)
+	{};
+	virtual ~BlockPDLimited() {};
+	float update(float input) {
+		float difference = 0.0f;
+		float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f);
+		getOutputLimiter().limit(output, difference);
+
+		return output;
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKD() { return _kD.get(); }
+	BlockDerivative &getDerivative() { return _derivative; }
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+private:
+	control::BlockParamFloat _kP;
+	control::BlockParamFloat _kD;
+	BlockDerivative _derivative;
+	BlockOutputLimiter _outputLimiter;
+};
+
+}
+
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
new file mode 100644
index 000000000..5b9238780
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -0,0 +1,419 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 mTecs_params.c
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/*
+ * Controller parameters, accessible via MAVLink
+ */
+
+/**
+ * mTECS enabled
+ *
+ * Set to 1 to enable mTECS
+ *
+ * @min 0
+ * @max 1
+ * @group mTECS
+ */
+PARAM_DEFINE_INT32(MT_ENABLED, 1);
+
+/**
+ * Total Energy Rate Control Feedforward
+ * Maps the total energy rate setpoint to the throttle setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f);
+
+/**
+ * Total Energy Rate Control P
+ * Maps the total energy rate error to the throttle setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f);
+
+/**
+ * Total Energy Rate Control I
+ * Maps the integrated total energy rate to the throttle setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f);
+
+/**
+ * Total Energy Rate Control Offset (Cruise throttle sp)
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
+
+/**
+ * Energy Distribution Rate Control Feedforward
+ * Maps the energy distribution rate setpoint to the pitch setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f);
+
+/**
+ * Energy Distribution Rate Control P
+ * Maps the energy distribution rate error to the pitch setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
+
+/**
+ * Energy Distribution Rate Control I
+ * Maps the integrated energy distribution rate error to the pitch setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
+
+
+/**
+ * Total Energy Distribution Offset (Cruise pitch sp)
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f);
+
+/**
+ * Minimal Throttle Setpoint
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f);
+
+/**
+ * Maximal Throttle Setpoint
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f);
+
+/**
+ * Minimal Pitch Setpoint in Degrees
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
+
+/**
+ * Maximal Pitch Setpoint in Degrees
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
+
+/**
+ * Lowpass (cutoff freq.) for the flight path angle
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
+
+/**
+ * P gain for the altitude control
+ * Maps the altitude error to the flight path angle setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f);
+
+/**
+ * D gain for the altitude control
+ * Maps the change of altitude error to the flight path angle setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
+
+/**
+ * Lowpass for FPA error derivative calculation (see MT_FPA_D)
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f);
+
+
+/**
+ * Minimal flight path angle setpoint
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f);
+
+/**
+ * Maximal flight path angle setpoint
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
+
+/**
+ * Lowpass (cutoff freq.) for airspeed
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
+
+/**
+ * P gain for the airspeed control
+ * Maps the airspeed error to the acceleration setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f);
+
+/**
+ * D gain for the airspeed control
+ * Maps the change of airspeed error to the acceleration setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
+
+/**
+ * Lowpass for ACC error derivative calculation (see MT_ACC_D)
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
+
+/**
+ * Minimal acceleration (air)
+ *
+ * @unit m/s^2
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
+
+/**
+ * Maximal acceleration (air)
+ *
+* @unit m/s^2
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
+
+/**
+ * Airspeed derivative calculation lowpass
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
+
+/**
+ * Minimal throttle during takeoff
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
+
+/**
+ * Maximal throttle during takeoff
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
+
+/**
+ * Minimal pitch during takeoff
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
+
+/**
+ * Maximal pitch during takeoff
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);
+
+/**
+ * Minimal throttle in underspeed mode
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f);
+
+/**
+ * Maximal throttle in underspeed mode
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f);
+
+/**
+ * Minimal pitch in underspeed mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f);
+
+/**
+ * Maximal pitch in underspeed mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f);
+
+/**
+ * Minimal throttle in landing mode (only last phase of landing)
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f);
+
+/**
+ * Maximal throttle in landing mode (only last phase of landing)
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f);
+
+/**
+ * Minimal pitch in landing mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f);
+
+/**
+ * Maximal pitch in landing mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f);
+
+/**
+ * Integrator Limit for Total Energy Rate Control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f);
+
+/**
+ * Integrator Limit for Energy Distribution Rate Control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f);
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index d383146f9..7758faed7 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -51,7 +51,6 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_armed.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
@@ -63,8 +62,6 @@ struct gpio_led_s {
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
-	struct actuator_armed_s armed;
-	int actuator_armed_sub;
 	bool led_state;
 	int counter;
 };
@@ -81,6 +78,7 @@ void gpio_led_cycle(FAR void *arg);
 int gpio_led_main(int argc, char *argv[])
 {
 	if (argc < 2) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
 		     "\t-p\tUse pin:\n"
 		     "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
@@ -88,7 +86,14 @@ int gpio_led_main(int argc, char *argv[])
 		     "\t\ta1\tPX4IO ACC1\n"
 		     "\t\ta2\tPX4IO ACC2\n"
 		     "\t\tr1\tPX4IO RELAY1\n"
-		     "\t\tr2\tPX4IO RELAY2");
+		     "\t\tr2\tPX4IO RELAY2"
+		    );
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
+		     "\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
+		    );
+#endif
 
 	} else {
 
@@ -98,37 +103,70 @@ int gpio_led_main(int argc, char *argv[])
 			}
 
 			bool use_io = false;
-			int pin = GPIO_EXT_1;
+
+			/* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */
+			int pin = 1;
+
+			/* pin name to display */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			char *pin_name = "PX4FMU GPIO_EXT1";
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+			char pin_name[] = "AUX OUT 1";
+#endif
 
 			if (argc > 2) {
 				if (!strcmp(argv[2], "-p")) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
 					if (!strcmp(argv[3], "1")) {
 						use_io = false;
 						pin = GPIO_EXT_1;
+						pin_name = "PX4FMU GPIO_EXT1";
 
 					} else if (!strcmp(argv[3], "2")) {
 						use_io = false;
 						pin = GPIO_EXT_2;
+						pin_name = "PX4FMU GPIO_EXT2";
 
 					} else if (!strcmp(argv[3], "a1")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_ACC1;
+						pin_name = "PX4IO ACC1";
 
 					} else if (!strcmp(argv[3], "a2")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_ACC2;
+						pin_name = "PX4IO ACC2";
 
 					} else if (!strcmp(argv[3], "r1")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_POWER1;
+						pin_name = "PX4IO RELAY1";
 
 					} else if (!strcmp(argv[3], "r2")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_POWER2;
+						pin_name = "PX4IO RELAY2";
+
+					} else {
+						errx(1, "unsupported pin: %s", argv[3]);
+					}
+
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+					unsigned int n = strtoul(argv[3], NULL, 10);
+
+					if (n >= 1 && n <= 6) {
+						use_io = false;
+						pin = 1 << (n - 1);
+						snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n);
 
 					} else {
 						errx(1, "unsupported pin: %s", argv[3]);
 					}
+
+#endif
 				}
 			}
 
@@ -142,32 +180,14 @@ int gpio_led_main(int argc, char *argv[])
 
 			} else {
 				gpio_led_started = true;
-				char pin_name[24];
-
-				if (use_io) {
-					if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
-						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
-
-					} else {
-						sprintf(pin_name, "PX4IO RELAY%i", pin);
-					}
-
-				} else {
-					sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
-
-				}
-
 				warnx("start, using pin: %s", pin_name);
+				exit(0);
 			}
-
-			exit(0);
-
-
 		} else if (!strcmp(argv[1], "stop")) {
 			if (gpio_led_started) {
 				gpio_led_started = false;
 				warnx("stop");
-
+				exit(0);
 			} else {
 				errx(1, "not running");
 			}
@@ -186,6 +206,7 @@ void gpio_led_start(FAR void *arg)
 
 	if (priv->use_io) {
 		gpio_dev = PX4IO_DEVICE_PATH;
+
 	} else {
 		gpio_dev = PX4FMU_DEVICE_PATH;
 	}
@@ -204,8 +225,10 @@ void gpio_led_start(FAR void *arg)
 	/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
 	ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
 
-	/* subscribe to vehicle status topic */
+	/* initialize vehicle status structure */
 	memset(&priv->status, 0, sizeof(priv->status));
+
+	/* subscribe to vehicle status topic */
 	priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* add worker to queue */
@@ -224,38 +247,33 @@ void gpio_led_cycle(FAR void *arg)
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
 	/* check for status updates*/
-	bool status_updated;
-	orb_check(priv->vehicle_status_sub, &status_updated);
+	bool updated;
+	orb_check(priv->vehicle_status_sub, &updated);
 
-	if (status_updated)
+	if (updated) {
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
-
-	orb_check(priv->vehicle_status_sub, &status_updated);
-
-	if (status_updated)
-		orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
+	}
 
 	/* select pattern for current status */
 	int pattern = 0;
 
-	if (priv->armed.armed) {
-		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+	if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		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) {
 			pattern = 0x3f;	// ****** solid (armed)
 
 		} else {
-			pattern = 0x2A;	// *_*_*_ fast blink (armed, battery warning)
+			pattern = 0x3e;	// *****_ slow blink (armed, battery low or failsafe)
 		}
 
-	} else {
-		if (priv->armed.ready_to_arm) {
-			pattern = 0x00;	// ______ off (disarmed, preflight check)
+	} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
+		pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
-		} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
-			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
+	} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
+		pattern = 0x28;	// *_*___ slow double blink (disarmed, error)
 
-		} else {
-			pattern = 0x28;	// *_*___ slow double blink (disarmed, not good to arm)
-		}
 	}
 
 	/* blink pattern */
@@ -266,6 +284,7 @@ void gpio_led_cycle(FAR void *arg)
 
 		if (led_state_new) {
 			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
+
 		} else {
 			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
 		}
@@ -273,8 +292,9 @@ void gpio_led_cycle(FAR void *arg)
 
 	priv->counter++;
 
-	if (priv->counter > 5)
+	if (priv->counter > 5) {
 		priv->counter = 0;
+	}
 
 	/* repeat cycle at 5 Hz */
 	if (gpio_led_started) {
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 20853379d..e49288a74 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.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
@@ -34,66 +33,41 @@
 
 /**
  * @file mavlink.c
- * MAVLink 1.0 protocol implementation.
+ * Adapter functions expected by the protocol library
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
 #include <unistd.h>
-#include <pthread.h>
 #include <stdio.h>
-#include <math.h>
 #include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
 #include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-#include <mavlink/mavlink_log.h>
-#include <commander/px4_custom_mode.h>
-
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "util.h"
-#include "waypoints.h"
-#include "mavlink_parameters.h"
 
 /* define MAVLink specific parameters */
+/**
+ * MAVLink system ID
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
+/**
+ * MAVLink component ID
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
+/**
+ * MAVLink type
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
-
-__EXPORT int mavlink_main(int argc, char *argv[]);
-
-static int mavlink_thread_main(int argc, char *argv[]);
-
-/* thread state */
-volatile bool thread_should_exit = false;
-static volatile bool thread_running = false;
-static int mavlink_task;
-
-/* pthreads */
-static pthread_t receive_thread;
-static pthread_t uorb_receive_thread;
-
-/* terminate MAVLink on user request - disabled by default */
-static bool mavlink_link_termination_allowed = false;
+/**
+ * Use/Accept HIL GPS message (even if not in HIL mode)
+ * If set to 1 incomming HIL GPS messages are parsed.
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
 
 mavlink_system_t mavlink_system = {
 	100,
@@ -104,357 +78,6 @@ mavlink_system_t mavlink_system = {
 	0
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
 
-/* XXX not widely used */
-uint8_t chan = MAVLINK_COMM_0;
-
-/* XXX probably should be in a header... */
-extern pthread_t receive_start(int uart);
-
-/* Allocate storage space for waypoints */
-static mavlink_wpm_storage wpm_s;
-mavlink_wpm_storage *wpm = &wpm_s;
-
-bool mavlink_hil_enabled = false;
-
-/* protocol interface */
-static int uart;
-static int baudrate;
-bool gcs_link = true;
-
-/* interface mode */
-static enum {
-	MAVLINK_INTERFACE_MODE_OFFBOARD,
-	MAVLINK_INTERFACE_MODE_ONBOARD
-} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
-
-static struct mavlink_logbuffer lb;
-
-static void mavlink_update_system(void);
-static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-static void usage(void);
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
-
-
-
-int
-set_hil_on_off(bool hil_enabled)
-{
-	int ret = OK;
-
-	/* Enable HIL */
-	if (hil_enabled && !mavlink_hil_enabled) {
-
-		mavlink_hil_enabled = true;
-
-		/* ramp up some HIL-related subscriptions */
-		unsigned hil_rate_interval;
-
-		if (baudrate < 19200) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (baudrate < 38400) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (baudrate < 115200) {
-			/* 20 Hz */
-			hil_rate_interval = 50;
-
-		} else {
-			/* 200 Hz */
-			hil_rate_interval = 5;
-		}
-
-		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
-	}
-
-	if (!hil_enabled && mavlink_hil_enabled) {
-		mavlink_hil_enabled = false;
-		orb_set_interval(mavlink_subs.spa_sub, 200);
-
-	} else {
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
-{
-	/* reset MAVLink mode bitfield */
-	*mavlink_base_mode = 0;
-	*mavlink_custom_mode = 0;
-
-	/**
-	 * Set mode flags
-	 **/
-
-	/* HIL */
-	if (v_status.hil_state == HIL_STATE_ON) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
-
-	/* arming state */
-	if (v_status.arming_state == ARMING_STATE_ARMED
-			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	/* main state */
-	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-	union px4_custom_mode custom_mode;
-	custom_mode.data = 0;
-	if (v_status.main_state == MAIN_STATE_MANUAL) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-	} else if (v_status.main_state == MAIN_STATE_EASY) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
-	} else if (v_status.main_state == MAIN_STATE_AUTO) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		}
-	}
-	*mavlink_custom_mode = custom_mode.data;
-
-	/**
-	 * Set mavlink state
-	 **/
-
-	/* set calibration state */
-	if (v_status.arming_state == ARMING_STATE_INIT
-			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
-			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
-		*mavlink_state = MAV_STATE_UNINIT;
-	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
-		*mavlink_state = MAV_STATE_ACTIVE;
-	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
-		*mavlink_state = MAV_STATE_CRITICAL;
-	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
-		*mavlink_state = MAV_STATE_STANDBY;
-	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
-		*mavlink_state = MAV_STATE_POWEROFF;
-	} else {
-		warnx("Unknown mavlink state");
-		*mavlink_state = MAV_STATE_CRITICAL;
-	}
-}
-
-
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
-{
-	int ret = OK;
-
-	switch (mavlink_msg_id) {
-	case MAVLINK_MSG_ID_SCALED_IMU:
-		/* sensor sub triggers scaled IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_HIGHRES_IMU:
-		/* sensor sub triggers highres IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_RAW_IMU:
-		/* sensor sub triggers RAW IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_ATTITUDE:
-		/* attitude sub triggers attitude */
-		orb_set_interval(subs->att_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
-		/* actuator_outputs triggers this message */
-		orb_set_interval(subs->act_0_sub, min_interval);
-		orb_set_interval(subs->act_1_sub, min_interval);
-		orb_set_interval(subs->act_2_sub, min_interval);
-		orb_set_interval(subs->act_3_sub, min_interval);
-		orb_set_interval(subs->actuators_sub, min_interval);
-		orb_set_interval(subs->actuators_effective_sub, min_interval);
-		orb_set_interval(subs->spa_sub, min_interval);
-		orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_MANUAL_CONTROL:
-		/* manual_control_setpoint triggers this message */
-		orb_set_interval(subs->man_control_sp_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
-		orb_set_interval(subs->debug_key_value, min_interval);
-		break;
-
-	default:
-		/* not found */
-		ret = ERROR;
-		break;
-	}
-
-	return ret;
-}
-
-
-/****************************************************************************
- * MAVLink text message logger
- ****************************************************************************/
-
-static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
-
-static const struct file_operations mavlink_fops = {
-	.ioctl = mavlink_dev_ioctl
-};
-
-static int
-mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
-{
-	static unsigned int total_counter = 0;
-
-	switch (cmd) {
-	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
-	case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
-	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
-			const char *txt = (const char *)arg;
-			struct mavlink_logmessage msg;
-			strncpy(msg.text, txt, sizeof(msg.text));
-			mavlink_logbuffer_write(&lb, &msg);
-			total_counter++;
-			return OK;
-		}
-
-	default:
-		return ENOTTY;
-	}
-}
-
-#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
-{
-	/* process baud rate */
-	int speed;
-
-	switch (baud) {
-	case 0:      speed = B0;      break;
-
-	case 50:     speed = B50;     break;
-
-	case 75:     speed = B75;     break;
-
-	case 110:    speed = B110;    break;
-
-	case 134:    speed = B134;    break;
-
-	case 150:    speed = B150;    break;
-
-	case 200:    speed = B200;    break;
-
-	case 300:    speed = B300;    break;
-
-	case 600:    speed = B600;    break;
-
-	case 1200:   speed = B1200;   break;
-
-	case 1800:   speed = B1800;   break;
-
-	case 2400:   speed = B2400;   break;
-
-	case 4800:   speed = B4800;   break;
-
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-	case 460800: speed = B460800; break;
-
-	case 921600: speed = B921600; break;
-
-	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
-		return -EINVAL;
-	}
-
-	/* open uart */
-	warnx("UART is %s, baudrate is %d\n", uart_name, baud);
-	uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-	*is_usb = false;
-
-	/* Back up the original uart configuration to restore it after exit */
-	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
-		close(uart);
-		return -1;
-	}
-
-	/* Fill the struct for the new configuration */
-	tcgetattr(uart, &uart_config);
-
-	/* Clear ONLCR flag (which appends a CR for every LF) */
-	uart_config.c_oflag &= ~ONLCR;
-
-	/* USB serial is indicated by /dev/ttyACM0*/
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
-
-	}
-
-	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-		close(uart);
-		return -1;
-	}
-
-	return uart;
-}
-
-void
-mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
-{
-	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
-}
-
 /*
  * Internal function to give access to the channel status for each channel
  */
@@ -472,347 +95,3 @@ extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
 	return &m_mavlink_buffer[channel];
 }
-
-void mavlink_update_system(void)
-{
-	static bool initialized = false;
-	static param_t param_system_id;
-	static param_t param_component_id;
-	static param_t param_system_type;
-
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-		initialized = true;
-	}
-
-	/* update system and component id */
-	int32_t system_id;
-	param_get(param_system_id, &system_id);
-
-	if (system_id > 0 && system_id < 255) {
-		mavlink_system.sysid = system_id;
-	}
-
-	int32_t component_id;
-	param_get(param_component_id, &component_id);
-
-	if (component_id > 0 && component_id < 255) {
-		mavlink_system.compid = component_id;
-	}
-
-	int32_t system_type;
-	param_get(param_system_type, &system_type);
-
-	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
-	}
-}
-
-/**
- * MAVLink Protocol main function.
- */
-int mavlink_thread_main(int argc, char *argv[])
-{
-	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 10);
-
-	int ch;
-	char *device_name = "/dev/ttyS1";
-	baudrate = 57600;
-
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-
-	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
-		switch (ch) {
-		case 'b':
-			baudrate = strtoul(optarg, NULL, 10);
-
-			if (baudrate < 9600 || baudrate > 921600)
-				errx(1, "invalid baud rate '%s'", optarg);
-
-			break;
-
-		case 'd':
-			device_name = optarg;
-			break;
-
-		case 'e':
-			mavlink_link_termination_allowed = true;
-			break;
-
-		case 'o':
-			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
-			break;
-
-		default:
-			usage();
-			break;
-		}
-	}
-
-	struct termios uart_config_original;
-
-	bool usb_uart;
-
-	/* print welcome text */
-	warnx("MAVLink v1.0 serial interface starting...");
-
-	/* inform about mode */
-	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
-
-	/* Flush stdout in case MAVLink is about to take it over */
-	fflush(stdout);
-
-	/* default values for arguments */
-	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
-
-	if (uart < 0)
-		err(1, "could not open %s", device_name);
-
-	/* create the device node that's used for sending text log messages, etc. */
-	register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
-
-	/* Initialize system properties */
-	mavlink_update_system();
-
-	/* start the MAVLink receiver */
-	receive_thread = receive_start(uart);
-
-	/* start the ORB receiver */
-	uorb_receive_thread = uorb_receive_start();
-
-	/* initialize waypoint manager */
-	mavlink_wpm_init(wpm);
-
-	/* all subscriptions are now active, set up initial guess about rate limits */
-	if (baudrate >= 230400) {
-		/* 200 Hz / 5 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
-		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
-		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
-		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
-		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
-
-	} else if (baudrate >= 115200) {
-		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-
-	} else if (baudrate >= 57600) {
-		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
-		/* 10 Hz / 100 ms ATTITUDE */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
-
-	} else {
-		/* very low baud rate, limit to 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
-		/* 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
-		/* 0.5 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
-		/* 0.1 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
-	}
-
-	thread_running = true;
-
-	/* arm counter to go off immediately */
-	unsigned lowspeed_counter = 10;
-
-	while (!thread_should_exit) {
-
-		/* 1 Hz */
-		if (lowspeed_counter == 10) {
-			mavlink_update_system();
-
-			/* translate the current system state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_base_mode = 0;
-			uint32_t mavlink_custom_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
-
-			/* switch HIL mode if required */
-			if (v_status.hil_state == HIL_STATE_ON)
-				set_hil_on_off(true);
-			else if (v_status.hil_state == HIL_STATE_OFF)
-				set_hil_on_off(false);
-
-			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(chan,
-						    v_status.onboard_control_sensors_present,
-						    v_status.onboard_control_sensors_enabled,
-						    v_status.onboard_control_sensors_health,
-						    v_status.load * 1000.0f,
-						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 100.0f,
-						    v_status.battery_remaining * 100.0f,
-						    v_status.drop_rate_comm,
-						    v_status.errors_comm,
-						    v_status.errors_count1,
-						    v_status.errors_count2,
-						    v_status.errors_count3,
-						    v_status.errors_count4);
-			lowspeed_counter = 0;
-		}
-
-		lowspeed_counter++;
-
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		/* check if waypoint has been reached against the last positions */
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		/* send parameters at 20 Hz (if queued for sending) */
-		mavlink_pm_queued_send();
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
-
-		if (baudrate > 57600) {
-			mavlink_pm_queued_send();
-		}
-
-		/* sleep 10 ms */
-		usleep(10000);
-
-		/* send one string at 10 Hz */
-		if (!mavlink_logbuffer_is_empty(&lb)) {
-			struct mavlink_logmessage msg;
-			int lb_ret = mavlink_logbuffer_read(&lb, &msg);
-
-			if (lb_ret == OK) {
-				mavlink_missionlib_send_gcs_string(msg.text);
-			}
-		}
-
-		/* sleep 15 ms */
-		usleep(15000);
-	}
-
-	/* wait for threads to complete */
-	pthread_join(receive_thread, NULL);
-	pthread_join(uorb_receive_thread, NULL);
-
-	/* Reset the UART flags to original state */
-	tcsetattr(uart, TCSANOW, &uart_config_original);
-
-	/* destroy log buffer */
-	//mavlink_logbuffer_destroy(&lb);
-
-	thread_running = false;
-
-	return 0;
-}
-
-static void
-usage()
-{
-	fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
-		"       mavlink stop\n"
-		"       mavlink status\n");
-	exit(1);
-}
-
-int mavlink_main(int argc, char *argv[])
-{
-
-	if (argc < 2) {
-		warnx("missing command");
-		usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		/* this is not an error */
-		if (thread_running)
-			errx(0, "mavlink already running");
-
-		thread_should_exit = false;
-		mavlink_task = task_spawn_cmd("mavlink",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  2048,
-					  mavlink_thread_main,
-					  (const char **)argv);
-
-		while (!thread_running) {
-			usleep(200);
-		}
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-
-		/* this is not an error */
-		if (!thread_running)
-			errx(0, "mavlink already stopped");
-
-		thread_should_exit = true;
-
-		while (thread_running) {
-			usleep(200000);
-			warnx(".");
-		}
-
-		warnx("terminated.");
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			errx(0, "running");
-
-		} else {
-			errx(1, "not running");
-		}
-	}
-
-	warnx("unrecognized command");
-	usage();
-	/* not getting here */
-	return 0;
-}
-
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 149efda60..374d1511c 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.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
@@ -43,6 +42,8 @@
 #ifndef MAVLINK_BRIDGE_HEADER_H
 #define MAVLINK_BRIDGE_HEADER_H
 
+__BEGIN_DECLS
+
 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
 /* use efficient approach, see mavlink_helpers.h */
@@ -73,11 +74,13 @@ extern mavlink_system_t mavlink_system;
  * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
  * @param ch Character to send
  */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
+void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
 #include <v1.0/common/mavlink.h>
 
+__END_DECLS
+
 #endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
new file mode 100644
index 000000000..fccd4d9a5
--- /dev/null
+++ b/src/modules/mavlink/mavlink_commands.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 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_time(0)
+{
+	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
+}
+
+void
+MavlinkCommandsStream::update(const hrt_abstime t)
+{
+	struct vehicle_command_s cmd;
+
+	if (_cmd_sub->update(&_cmd_time, &cmd)) {
+		/* only send commands for other systems/components */
+		if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
+			mavlink_msg_command_long_send(_channel,
+						      cmd.target_system,
+						      cmd.target_component,
+						      cmd.command,
+						      cmd.confirmation,
+						      cmd.param1,
+						      cmd.param2,
+						      cmd.param3,
+						      cmd.param4,
+						      cmd.param5,
+						      cmd.param6,
+						      cmd.param7);
+		}
+	}
+}
diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h
new file mode 100644
index 000000000..abdba34b9
--- /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;
+	uint64_t _cmd_time;
+
+public:
+	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
+	void update(const hrt_abstime t);
+};
+
+#endif /* MAVLINK_COMMANDS_H_ */
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
new file mode 100644
index 000000000..675a6870e
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -0,0 +1,423 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <crc32.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include "mavlink_ftp.h"
+
+MavlinkFTP *MavlinkFTP::_server;
+
+MavlinkFTP *
+MavlinkFTP::getServer()
+{
+	// XXX this really cries out for some locking...
+	if (_server == nullptr) {
+		_server = new MavlinkFTP;
+	}
+	return _server;
+}
+
+MavlinkFTP::MavlinkFTP()
+{
+	// initialise the request freelist
+	dq_init(&_workFree);
+	sem_init(&_lock, 0, 1);
+    
+    // initialize session list
+    for (size_t i=0; i<kMaxSession; i++) {
+        _session_fds[i] = -1;
+    }
+
+	// drop work entries onto the free list
+	for (unsigned i = 0; i < kRequestQueueSize; i++) {
+		_qFree(&_workBufs[i]);
+	}
+
+}
+
+void
+MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
+{
+	// get a free request
+	auto req = _dqFree();
+
+	// if we couldn't get a request slot, just drop it
+	if (req != nullptr) {
+
+		// decode the request
+		if (req->decode(mavlink, msg)) {
+
+			// and queue it for the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+		} else {
+			_qFree(req);
+		}
+	}
+}
+
+void
+MavlinkFTP::_workerTrampoline(void *arg)
+{
+	auto req = reinterpret_cast<Request *>(arg);
+	auto server = MavlinkFTP::getServer();
+
+	// call the server worker with the work item
+	server->_worker(req);
+}
+
+void
+MavlinkFTP::_worker(Request *req)
+{
+	auto hdr = req->header();
+	ErrorCode errorCode = kErrNone;
+	uint32_t messageCRC;
+
+	// basic sanity checks; must validate length before use
+	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+		errorCode = kErrNoRequest;
+		goto out;
+	}
+
+	// check request CRC to make sure this is one of ours
+	messageCRC = hdr->crc32;
+	hdr->crc32 = 0;
+	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
+		errorCode = kErrNoRequest;
+		goto out;
+		warnx("ftp: bad crc");
+	}
+
+	//printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+
+	switch (hdr->opcode) {
+	case kCmdNone:
+		break;
+
+	case kCmdTerminate:
+		errorCode = _workTerminate(req);
+		break;
+
+	case kCmdReset:
+		errorCode = _workReset();
+		break;
+
+	case kCmdList:
+		errorCode = _workList(req);
+		break;
+
+	case kCmdOpen:
+		errorCode = _workOpen(req, false);
+		break;
+
+	case kCmdCreate:
+		errorCode = _workOpen(req, true);
+		break;
+
+	case kCmdRead:
+		errorCode = _workRead(req);
+		break;
+
+	case kCmdWrite:
+		errorCode = _workWrite(req);
+		break;
+
+	case kCmdRemove:
+		errorCode = _workRemove(req);
+		break;
+
+	default:
+		errorCode = kErrNoRequest;
+		break;	
+	}
+
+out:
+	// handle success vs. error
+	if (errorCode == kErrNone) {
+		hdr->opcode = kRspAck;
+		//warnx("FTP: ack\n");
+	} else {
+		warnx("FTP: nak %u", errorCode);
+		hdr->opcode = kRspNak;
+		hdr->size = 1;
+		hdr->data[0] = errorCode;
+	}
+
+	// respond to the request
+	_reply(req);
+
+	// free the request buffer back to the freelist
+	_qFree(req);
+}
+
+void
+MavlinkFTP::_reply(Request *req)
+{
+	auto hdr = req->header();
+
+	// message is assumed to be already constructed in the request buffer, so generate the CRC
+	hdr->crc32 = 0;
+	hdr->crc32 = crc32(req->rawData(), req->dataSize());
+
+	// then pack and send the reply back to the request source
+	req->reply();
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workList(Request *req)
+{
+	auto hdr = req->header();
+    
+    char dirPath[kMaxDataLength];
+    strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
+    
+	DIR *dp = opendir(dirPath);
+
+	if (dp == nullptr) {
+		warnx("FTP: can't open path '%s'", dirPath);
+		return kErrNotDir;
+	}
+    
+	//warnx("FTP: list %s offset %d", dirPath, hdr->offset);
+
+	ErrorCode errorCode = kErrNone;
+	struct dirent entry, *result = nullptr;
+	unsigned offset = 0;
+
+	// move to the requested offset
+	seekdir(dp, hdr->offset);
+
+	for (;;) {
+		// read the directory entry
+		if (readdir_r(dp, &entry, &result)) {
+			warnx("FTP: list %s readdir_r failure\n", dirPath);
+			errorCode = kErrIO;
+			break;
+		}
+
+		// no more entries?
+		if (result == nullptr) {
+			if (hdr->offset != 0 && offset == 0) {
+				// User is requesting subsequent dir entries but there were none. This means the user asked
+				// to seek past EOF.
+				errorCode = kErrEOF;
+			}
+			// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
+			break;
+		}
+
+		// name too big to fit?
+		if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
+			break;
+		}
+
+		// store the type marker
+		switch (entry.d_type) {
+		case DTYPE_FILE:
+			hdr->data[offset++] = kDirentFile;
+			break;
+		case DTYPE_DIRECTORY:
+			hdr->data[offset++] = kDirentDir;
+			break;
+		default:
+			hdr->data[offset++] = kDirentUnknown;
+			break;
+		}
+
+		// copy the name, which we know will fit
+		strcpy((char *)&hdr->data[offset], entry.d_name);
+		//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
+		offset += strlen(entry.d_name) + 1;
+	}
+
+	closedir(dp);
+	hdr->size = offset;
+
+	return errorCode;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workOpen(Request *req, bool create)
+{
+	auto hdr = req->header();
+    
+	int session_index = _findUnusedSession();
+	if (session_index < 0) {
+		return kErrNoSession;
+	}
+
+	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
+    
+	int fd = ::open(req->dataAsCString(), oflag);
+	if (fd < 0) {
+		return create ? kErrPerm : kErrNotFile;
+	}
+	_session_fds[session_index] = fd;
+
+	hdr->session = session_index;
+	hdr->size = 0;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRead(Request *req)
+{
+	auto hdr = req->header();
+
+	int session_index = hdr->session;
+
+	if (!_validSession(session_index)) {
+		return kErrNoSession;
+	}
+
+	// Seek to the specified position
+	//warnx("seek %d", hdr->offset);
+	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
+		// Unable to see to the specified location
+		warnx("seek fail");
+		return kErrEOF;
+	}
+
+	int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
+	if (bytes_read < 0) {
+		// Negative return indicates error other than eof
+		warnx("read fail %d", bytes_read);
+		return kErrIO;
+	}
+
+	hdr->size = bytes_read;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workWrite(Request *req)
+{
+#if 0
+    // NYI: Coming soon
+	auto hdr = req->header();
+
+	// look up session
+	auto session = getSession(hdr->session);
+	if (session == nullptr) {
+		return kErrNoSession;
+	}
+
+	// append to file
+	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+
+	if (result < 0) {
+		// XXX might also be no space, I/O, etc.
+		return kErrNotAppend;
+	}
+
+	hdr->size = result;
+	return kErrNone;
+#else
+	return kErrPerm;
+#endif
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemove(Request *req)
+{
+	//auto hdr = req->header();
+
+	// for now, send error reply
+	return kErrPerm;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTerminate(Request *req)
+{
+	auto hdr = req->header();
+    
+	if (!_validSession(hdr->session)) {
+		return kErrNoSession;
+	}
+    
+	::close(_session_fds[hdr->session]);
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workReset(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] != -1) {
+			::close(_session_fds[i]);
+			_session_fds[i] = -1;
+		}
+	}
+
+	return kErrNone;
+}
+
+bool
+MavlinkFTP::_validSession(unsigned index)
+{
+	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
+		return false;
+	}
+	return true;
+}
+
+int
+MavlinkFTP::_findUnusedSession(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] == -1) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+char *
+MavlinkFTP::Request::dataAsCString()
+{
+	// guarantee nul termination
+	if (header()->size < kMaxDataLength) {
+		requestData()[header()->size] = '\0';
+	} else {
+		requestData()[kMaxDataLength - 1] = '\0';
+	}
+
+	// and return data
+	return (char *)&(header()->data[0]);
+}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
new file mode 100644
index 000000000..59237a4c4
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @file mavlink_ftp.h
+ *
+ * MAVLink remote file server.
+ *
+ * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
+ * a session ID and sequence number.
+ *
+ * A limited number of requests (currently 2) may be outstanding at a time.
+ * Additional messages will be discarded.
+ *
+ * Messages consist of a fixed header, followed by a data area.
+ *
+ */
+ 
+#include <dirent.h>
+#include <queue.h>
+
+#include <nuttx/wqueue.h>
+#include <systemlib/err.h>
+
+#include "mavlink_messages.h"
+
+class MavlinkFTP
+{
+public:
+	MavlinkFTP();
+
+	static MavlinkFTP	*getServer();
+
+	// static interface
+	void			handle_message(Mavlink* mavlink,
+					mavlink_message_t *msg);
+
+private:
+
+	static const unsigned	kRequestQueueSize = 2;
+
+	static MavlinkFTP	*_server;
+
+	struct RequestHeader
+	{
+		uint8_t		magic;
+		uint8_t		session;
+		uint8_t		opcode;
+		uint8_t		size;
+		uint32_t	crc32;
+		uint32_t	offset;
+		uint8_t		data[];
+	};
+
+	enum Opcode : uint8_t
+	{
+		kCmdNone,	// ignored, always acked
+		kCmdTerminate,	// releases sessionID, closes file
+		kCmdReset,	// terminates all sessions
+		kCmdList,	// list files in <path> from <offset>
+		kCmdOpen,	// opens <path> for reading, returns <session>
+		kCmdRead,	// reads <size> bytes from <offset> in <session>
+		kCmdCreate,	// creates <path> for writing, returns <session>
+		kCmdWrite,	// appends <size> bytes at <offset> in <session>
+		kCmdRemove,	// remove file (only if created by server?)
+
+		kRspAck,
+		kRspNak
+	};
+
+	enum ErrorCode : uint8_t
+	{
+		kErrNone,
+		kErrNoRequest,
+		kErrNoSession,
+		kErrSequence,
+		kErrNotDir,
+		kErrNotFile,
+		kErrEOF,
+		kErrNotAppend,
+		kErrTooBig,
+		kErrIO,
+		kErrPerm
+	};
+
+    int _findUnusedSession(void);
+    bool _validSession(unsigned index);
+
+    static const unsigned kMaxSession = 2;
+    int	_session_fds[kMaxSession];
+
+	class Request
+	{
+	public:
+		union {
+			dq_entry_t	entry;
+			work_s		work;
+		};
+
+		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+				_mavlink = mavlink;
+				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+				return true;
+			}
+			return false;
+		}
+
+		void		reply() {
+
+			// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
+			// flat memory architecture, as we're operating between threads here.
+			mavlink_message_t msg;
+			msg.checksum = 0;
+			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+				_mavlink->get_channel(), &msg, sequence(), rawData());
+
+			_mavlink->lockMessageBufferMutex();
+			bool success = _mavlink->message_buffer_write(&msg, len);
+			_mavlink->unlockMessageBufferMutex();
+
+			if (!success) {
+				warnx("FTP TX ERR");
+			} 
+			// else {
+				// warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+				// 	_mavlink->get_system_id(),
+				// 	_mavlink->get_component_id(),
+				// 	_mavlink->get_channel(),
+				// 	len,
+				// 	msg.checksum);
+			// }
+		}
+
+		uint8_t		*rawData() { return &_message.data[0]; }
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		uint8_t         *requestData() { return &(header()->data[0]); }
+		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
+		uint16_t	sequence() const { return _message.seqnr; }
+		mavlink_channel_t channel() { return _mavlink->get_channel(); }
+
+		char		*dataAsCString();
+
+	private:
+		Mavlink			*_mavlink;
+		mavlink_encapsulated_data_t _message;
+
+	};
+
+	static const uint8_t	kProtocolMagic = 'f';
+	static const char	kDirentFile = 'F';
+	static const char	kDirentDir = 'D';
+	static const char	kDirentUnknown = 'U';
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+
+	/// Request worker; runs on the low-priority work queue to service
+	/// remote requests.
+	///
+	static void		_workerTrampoline(void *arg);
+	void			_worker(Request *req);
+
+	/// Reply to a request (XXX should be a Request method)
+	///
+	void			_reply(Request *req);
+
+	ErrorCode		_workList(Request *req);
+	ErrorCode		_workOpen(Request *req, bool create);
+	ErrorCode		_workRead(Request *req);
+	ErrorCode		_workWrite(Request *req);
+	ErrorCode		_workRemove(Request *req);
+	ErrorCode		_workTerminate(Request *req);
+	ErrorCode		_workReset();
+
+	// work freelist
+	Request			_workBufs[kRequestQueueSize];
+	dq_queue_t		_workFree;
+	sem_t			_lock;
+
+	void			_qLock() { do {} while (sem_wait(&_lock) != 0); }
+	void			_qUnlock() { sem_post(&_lock); }
+
+	void			_qFree(Request *req) {
+		_qLock();
+		dq_addlast(&req->entry, &_workFree);
+		_qUnlock();
+	}
+
+	Request			*_dqFree() {
+		_qLock();
+		auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
+		_qUnlock();
+		return req;
+	}
+    
+};
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
new file mode 100644
index 000000000..cd0581af4
--- /dev/null
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -0,0 +1,1759 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * 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_main.cpp
+ * MAVLink 1.0 protocol implementation.
+ *
+ * @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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <assert.h>
+#include <math.h>
+#include <poll.h>
+#include <termios.h>
+#include <time.h>
+#include <math.h> /* isinf / isnan checks */
+
+#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 <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 <uORB/topics/parameter_update.h>
+
+#include "mavlink_bridge_header.h"
+#include "mavlink_main.h"
+#include "mavlink_messages.h"
+#include "mavlink_receiver.h"
+#include "mavlink_rate_limiter.h"
+#include "mavlink_commands.h"
+
+#ifndef MAVLINK_CRC_EXTRA
+ #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
+#endif
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#define DEFAULT_DEVICE_NAME	"/dev/ttyS1"
+#define MAX_DATA_RATE	10000	// max data rate in bytes/s
+#define MAIN_LOOP_DELAY 10000	// 100 Hz @ 1000 bytes/s data rate
+
+static Mavlink *_mavlink_instances = nullptr;
+
+/* TODO: if this is a class member it crashes */
+static struct file_operations fops;
+
+/**
+ * mavlink app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
+
+extern mavlink_system_t mavlink_system;
+
+static uint64_t last_write_success_times[6] = {0};
+static uint64_t last_write_try_times[6] = {0};
+
+/*
+ * Internal function to send the bytes through the right serial port
+ */
+void
+mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
+{
+
+	Mavlink *instance;
+
+	switch (channel) {
+	case MAVLINK_COMM_0:
+		instance = Mavlink::get_instance(0);
+		break;
+
+	case MAVLINK_COMM_1:
+		instance = Mavlink::get_instance(1);
+		break;
+
+	case MAVLINK_COMM_2:
+		instance = Mavlink::get_instance(2);
+		break;
+
+	case MAVLINK_COMM_3:
+		instance = Mavlink::get_instance(3);
+		break;
+#ifdef MAVLINK_COMM_4
+
+	case MAVLINK_COMM_4:
+		instance = Mavlink::get_instance(4);
+		break;
+#endif
+#ifdef MAVLINK_COMM_5
+
+	case MAVLINK_COMM_5:
+		instance = Mavlink::get_instance(5);
+		break;
+#endif
+#ifdef MAVLINK_COMM_6
+
+	case MAVLINK_COMM_6:
+		instance = Mavlink::get_instance(6);
+		break;
+#endif
+		default:
+		return;
+	}
+
+	int uart = instance->get_uart_fd();
+
+	ssize_t desired = (sizeof(uint8_t) * length);
+
+	/*
+	 * Check if the OS buffer is full and disable HW
+	 * flow control if it continues to be full
+	 */
+	int buf_free = 0;
+
+	if (instance->get_flow_control_enabled()
+		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+
+		/* Disable hardware flow control:
+		 * if no successful write since a defined time
+		 * and if the last try was not the last successful write
+		 */
+		if (last_write_try_times[(unsigned)channel] != 0 &&
+			hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
+			last_write_success_times[(unsigned)channel] !=
+			last_write_try_times[(unsigned)channel])
+		{
+			warnx("DISABLING HARDWARE FLOW CONTROL");
+			instance->enable_flow_control(false);
+		}
+
+	}
+
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (instance->should_transmit()) {
+		last_write_try_times[(unsigned)channel] = hrt_absolute_time();
+
+		/* check if there is space in the buffer, let it overflow else */
+		if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
+
+			if (buf_free < desired) {
+				/* we don't want to send anything just in half, so return */
+				instance->count_txerr();
+				return;
+			}
+		}
+
+		ssize_t ret = write(uart, ch, desired);
+		if (ret != desired) {
+			instance->count_txerr();
+		} else {
+			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
+		}
+	}
+}
+
+static void usage(void);
+
+Mavlink::Mavlink() :
+	_device_name(DEFAULT_DEVICE_NAME),
+	_task_should_exit(false),
+	next(nullptr),
+	_mavlink_fd(-1),
+	_task_running(false),
+	_hil_enabled(false),
+	_use_hil_gps(false),
+	_is_usb_uart(false),
+	_wait_to_transmit(false),
+	_received_messages(false),
+	_main_loop_delay(1000),
+	_subscriptions(nullptr),
+	_streams(nullptr),
+	_mission_manager(nullptr),
+	_mission_pub(-1),
+	_mission_result_sub(-1),
+	_mode(MAVLINK_MODE_NORMAL),
+	_total_counter(0),
+	_verbose(false),
+	_forwarding_on(false),
+	_passing_on(false),
+	_ftp_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({}),
+	_param_initialized(false),
+	_param_system_id(0),
+	_param_component_id(0),
+	_param_system_type(0),
+	_param_use_hil_gps(0),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
+{
+	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
+
+	_instance_id = Mavlink::instance_count();
+
+	/* set channel according to instance id */
+	switch (_instance_id) {
+	case 0:
+		_channel = MAVLINK_COMM_0;
+		break;
+
+	case 1:
+		_channel = MAVLINK_COMM_1;
+		break;
+
+	case 2:
+		_channel = MAVLINK_COMM_2;
+		break;
+
+	case 3:
+		_channel = MAVLINK_COMM_3;
+		break;
+#ifdef MAVLINK_COMM_4
+
+	case 4:
+		_channel = MAVLINK_COMM_4;
+		break;
+#endif
+#ifdef MAVLINK_COMM_5
+
+	case 5:
+		_channel = MAVLINK_COMM_5;
+		break;
+#endif
+#ifdef MAVLINK_COMM_6
+
+	case 6:
+		_channel = MAVLINK_COMM_6;
+		break;
+#endif
+
+	default:
+		errx(1, "instance ID is out of range");
+		break;
+	}
+}
+
+Mavlink::~Mavlink()
+{
+	perf_free(_loop_perf);
+	perf_free(_txerr_perf);
+
+	if (_task_running) {
+		/* task wakes up every 10ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				//TODO store main task handle in Mavlink instance to allow killing task
+				//task_delete(_mavlink_task);
+				break;
+			}
+		} while (_task_running);
+	}
+
+	LL_DELETE(_mavlink_instances, this);
+}
+
+void
+Mavlink::count_txerr()
+{
+	perf_count(_txerr_perf);
+}
+
+void
+Mavlink::set_mode(enum MAVLINK_MODE mode)
+{
+	_mode = mode;
+}
+
+int
+Mavlink::instance_count()
+{
+	unsigned inst_index = 0;
+	Mavlink *inst;
+
+	LL_FOREACH(::_mavlink_instances, inst) {
+		inst_index++;
+	}
+
+	return inst_index;
+}
+
+Mavlink *
+Mavlink::get_instance(unsigned instance)
+{
+	Mavlink *inst;
+	unsigned inst_index = 0;
+	LL_FOREACH(::_mavlink_instances, inst) {
+		if (instance == inst_index) {
+			return inst;
+		}
+
+		inst_index++;
+	}
+
+	return nullptr;
+}
+
+Mavlink *
+Mavlink::get_instance_for_device(const char *device_name)
+{
+	Mavlink *inst;
+
+	LL_FOREACH(::_mavlink_instances, inst) {
+		if (strcmp(inst->_device_name, device_name) == 0) {
+			return inst;
+		}
+	}
+
+	return nullptr;
+}
+
+int
+Mavlink::destroy_all_instances()
+{
+	/* start deleting from the end */
+	Mavlink *inst_to_del = nullptr;
+	Mavlink *next_inst = ::_mavlink_instances;
+
+	unsigned iterations = 0;
+
+	warnx("waiting for instances to stop");
+
+	while (next_inst != nullptr) {
+		inst_to_del = next_inst;
+		next_inst = inst_to_del->next;
+
+		/* set flag to stop thread and wait for all threads to finish */
+		inst_to_del->_task_should_exit = true;
+
+		while (inst_to_del->_task_running) {
+			printf(".");
+			fflush(stdout);
+			usleep(10000);
+			iterations++;
+
+			if (iterations > 1000) {
+				warnx("ERROR: Couldn't stop all mavlink instances.");
+				return ERROR;
+			}
+		}
+	}
+
+	printf("\n");
+	warnx("all instances stopped");
+	return OK;
+}
+
+bool
+Mavlink::instance_exists(const char *device_name, Mavlink *self)
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	while (inst != nullptr) {
+
+		/* don't compare with itself */
+		if (inst != self && !strcmp(device_name, inst->_device_name)) {
+			return true;
+		}
+
+		inst = inst->next;
+	}
+
+	return false;
+}
+
+void
+Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
+{
+
+	Mavlink *inst;
+	LL_FOREACH(_mavlink_instances, inst) {
+		if (inst != self) {
+			inst->pass_message(msg);
+		}
+	}
+}
+
+int
+Mavlink::get_uart_fd(unsigned index)
+{
+	Mavlink *inst = get_instance(index);
+
+	if (inst) {
+		return inst->get_uart_fd();
+	}
+
+	return -1;
+}
+
+int
+Mavlink::get_uart_fd()
+{
+	return _uart_fd;
+}
+
+int
+Mavlink::get_instance_id()
+{
+	return _instance_id;
+}
+
+mavlink_channel_t
+Mavlink::get_channel()
+{
+	return _channel;
+}
+
+/****************************************************************************
+ * MAVLink text message logger
+ ****************************************************************************/
+
+int
+Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
+	case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
+	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
+
+			const char *txt = (const char *)arg;
+//		printf("logmsg: %s\n", txt);
+			struct mavlink_logmessage msg;
+			strncpy(msg.text, txt, sizeof(msg.text));
+			msg.severity = (unsigned char)cmd;
+
+			Mavlink *inst;
+			LL_FOREACH(_mavlink_instances, inst) {
+				if (!inst->_task_should_exit) {
+					mavlink_logbuffer_write(&inst->_logbuffer, &msg);
+					inst->_total_counter++;
+				}
+			}
+
+			return OK;
+		}
+
+	default:
+		return ENOTTY;
+	}
+}
+
+void Mavlink::mavlink_update_system(void)
+{
+	if (!_param_initialized) {
+		_param_system_id = param_find("MAV_SYS_ID");
+		_param_component_id = param_find("MAV_COMP_ID");
+		_param_system_type = param_find("MAV_TYPE");
+		_param_use_hil_gps = param_find("MAV_USEHILGPS");
+		_param_initialized = true;
+	}
+
+	/* update system and component id */
+	int32_t system_id;
+	param_get(_param_system_id, &system_id);
+
+	if (system_id > 0 && system_id < 255) {
+		mavlink_system.sysid = system_id;
+	}
+
+	int32_t component_id;
+	param_get(_param_component_id, &component_id);
+
+	if (component_id > 0 && component_id < 255) {
+		mavlink_system.compid = component_id;
+	}
+
+	int32_t system_type;
+	param_get(_param_system_type, &system_type);
+
+	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
+		mavlink_system.type = system_type;
+	}
+
+	int32_t use_hil_gps;
+	param_get(_param_use_hil_gps, &use_hil_gps);
+
+	_use_hil_gps = (bool)use_hil_gps;
+}
+
+int Mavlink::get_system_id()
+{
+	return mavlink_system.sysid;
+}
+
+int Mavlink::get_component_id()
+{
+	return mavlink_system.compid;
+}
+
+int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
+{
+	/* process baud rate */
+	int speed;
+
+	switch (baud) {
+	case 0:      speed = B0;      break;
+
+	case 50:     speed = B50;     break;
+
+	case 75:     speed = B75;     break;
+
+	case 110:    speed = B110;    break;
+
+	case 134:    speed = B134;    break;
+
+	case 150:    speed = B150;    break;
+
+	case 200:    speed = B200;    break;
+
+	case 300:    speed = B300;    break;
+
+	case 600:    speed = B600;    break;
+
+	case 1200:   speed = B1200;   break;
+
+	case 1800:   speed = B1800;   break;
+
+	case 2400:   speed = B2400;   break;
+
+	case 4800:   speed = B4800;   break;
+
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+	case 460800: speed = B460800; break;
+
+	case 921600: speed = B921600; break;
+
+	default:
+		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud);
+		return -EINVAL;
+	}
+
+	/* open uart */
+	_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
+
+	if (_uart_fd < 0) {
+		return _uart_fd;
+	}
+
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+	*is_usb = false;
+
+	/* Back up the original uart configuration to restore it after exit */
+	if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
+		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
+		close(_uart_fd);
+		return -1;
+	}
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(_uart_fd, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* USB serial is indicated by /dev/ttyACM0*/
+	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
+
+		/* Set baud rate */
+		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+			warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
+			close(_uart_fd);
+			return -1;
+		}
+
+	}
+
+	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR SET CONF %s\n", uart_name);
+		close(_uart_fd);
+		return -1;
+	}
+
+	if (!_is_usb_uart) {
+		/*
+		 * Setup hardware flow control. If the port has no RTS pin this call will fail,
+		 * which is not an issue, but requires a separate call so we can fail silently.
+		 */
+		(void)tcgetattr(_uart_fd, &uart_config);
+		uart_config.c_cflag |= CRTS_IFLOW;
+		(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
+
+		/* setup output flow control */
+		if (enable_flow_control(true)) {
+			warnx("hardware flow control not supported");
+		}
+	}
+
+	return _uart_fd;
+}
+
+int
+Mavlink::enable_flow_control(bool enabled)
+{
+	// We can't do this on USB - skip
+	if (_is_usb_uart) {
+		return OK;
+	}
+
+	struct termios uart_config;
+
+	int ret = tcgetattr(_uart_fd, &uart_config);
+
+	if (enabled) {
+		uart_config.c_cflag |= CRTSCTS;
+
+	} else {
+		uart_config.c_cflag &= ~CRTSCTS;
+	}
+
+	ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
+
+	if (!ret) {
+		_flow_control_enabled = enabled;
+	}
+
+	return ret;
+}
+
+int
+Mavlink::set_hil_enabled(bool hil_enabled)
+{
+	int ret = OK;
+
+	/* enable HIL */
+	if (hil_enabled && !_hil_enabled) {
+		_hil_enabled = true;
+		float rate_mult = _datarate / 1000.0f;
+		configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
+	}
+
+	/* disable HIL */
+	if (!hil_enabled && _hil_enabled) {
+		_hil_enabled = false;
+		configure_stream("HIL_CONTROLS", 0.0f);
+
+	} else {
+		ret = ERROR;
+	}
+
+	return ret;
+}
+
+void
+Mavlink::send_message(const mavlink_message_t *msg)
+{
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+	mavlink_send_uart_bytes(_channel, buf, len);
+}
+
+void
+Mavlink::handle_message(const mavlink_message_t *msg)
+{
+	/* handle packet with mission manager */
+	_mission_manager->handle_message(msg);
+
+	/* handle packet with parameter component */
+	mavlink_pm_message_handler(_channel, msg);
+
+	if (get_forwarding_on()) {
+		/* forward any messages to other mavlink instances */
+		Mavlink::forward_message(msg, this);
+	}
+}
+
+int
+Mavlink::mavlink_pm_queued_send()
+{
+	if (_mavlink_param_queue_index < param_count()) {
+		mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
+		_mavlink_param_queue_index++;
+		return 0;
+
+	} else {
+		return 1;
+	}
+}
+
+void Mavlink::mavlink_pm_start_queued_send()
+{
+	_mavlink_param_queue_index = 0;
+}
+
+int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
+{
+	return mavlink_pm_send_param(param_for_index(index));
+}
+
+int Mavlink::mavlink_pm_send_param_for_name(const char *name)
+{
+	return mavlink_pm_send_param(param_find(name));
+}
+
+int Mavlink::mavlink_pm_send_param(param_t param)
+{
+	if (param == PARAM_INVALID) { return 1; }
+
+	/* buffers for param transmission */
+	char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+	float val_buf;
+	mavlink_message_t tx_msg;
+
+	/* query parameter type */
+	param_type_t type = param_type(param);
+	/* copy parameter name */
+	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+
+	/*
+	 * Map onboard parameter type to MAVLink type,
+	 * endianess matches (both little endian)
+	 */
+	uint8_t mavlink_type;
+
+	if (type == PARAM_TYPE_INT32) {
+		mavlink_type = MAVLINK_TYPE_INT32_T;
+
+	} else if (type == PARAM_TYPE_FLOAT) {
+		mavlink_type = MAVLINK_TYPE_FLOAT;
+
+	} else {
+		mavlink_type = MAVLINK_TYPE_FLOAT;
+	}
+
+	/*
+	 * get param value, since MAVLink encodes float and int params in the same
+	 * space during transmission, copy param onto float val_buf
+	 */
+
+	int ret;
+
+	if ((ret = param_get(param, &val_buf)) != OK) {
+		return ret;
+	}
+
+	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
+					  mavlink_system.compid,
+					  _channel,
+					  &tx_msg,
+					  name_buf,
+					  val_buf,
+					  mavlink_type,
+					  param_count(),
+					  param_get_index(param));
+	send_message(&tx_msg);
+	return OK;
+}
+
+void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+			mavlink_param_request_list_t req;
+			mavlink_msg_param_request_list_decode(msg, &req);
+			if (req.target_system == mavlink_system.sysid &&
+					(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
+				/* Start sending parameters */
+				mavlink_pm_start_queued_send();
+				send_statustext_info("[pm] sending list");
+			}
+		} break;
+
+	case MAVLINK_MSG_ID_PARAM_SET: {
+
+			/* Handle parameter setting */
+
+			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+				mavlink_param_set_t mavlink_param_set;
+				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
+
+				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter, set and send it */
+					param_t param = param_find(name);
+
+					if (param == PARAM_INVALID) {
+						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
+						sprintf(buf, "[pm] unknown: %s", name);
+						send_statustext_info(buf);
+
+					} else {
+						/* set and send parameter */
+						param_set(param, &(mavlink_param_set.param_value));
+						mavlink_pm_send_param(param);
+					}
+				}
+			}
+		} break;
+
+	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+			mavlink_param_request_read_t mavlink_param_request_read;
+			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
+
+			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
+				/* when no index is given, loop through string ids and compare them */
+				if (mavlink_param_request_read.param_index == -1) {
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+					strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter and send it */
+					mavlink_pm_send_param_for_name(name);
+
+				} else {
+					/* when index is >= 0, send this parameter again */
+					mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
+				}
+			}
+
+		} break;
+	}
+}
+
+int
+Mavlink::send_statustext_info(const char *string)
+{
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
+}
+
+int
+Mavlink::send_statustext_critical(const char *string)
+{
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
+}
+
+int
+Mavlink::send_statustext_emergency(const char *string)
+{
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
+}
+
+int
+Mavlink::send_statustext(unsigned severity, const char *string)
+{
+	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
+	mavlink_statustext_t statustext;
+
+	int i = 0;
+
+	while (i < len - 1) {
+		statustext.text[i] = string[i];
+
+		if (string[i] == '\0') {
+			break;
+		}
+
+		i++;
+	}
+
+	if (i > 1) {
+		/* Enforce null termination */
+		statustext.text[i] = '\0';
+
+		/* Map severity */
+		switch (severity) {
+			case MAVLINK_IOC_SEND_TEXT_INFO:
+				statustext.severity = MAV_SEVERITY_INFO;
+				break;
+			case MAVLINK_IOC_SEND_TEXT_CRITICAL:
+				statustext.severity = MAV_SEVERITY_CRITICAL;
+				break;
+			case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
+				statustext.severity = MAV_SEVERITY_EMERGENCY;
+				break;
+		}
+
+		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
+		return OK;
+
+	} else {
+		return ERROR;
+	}
+}
+
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
+{
+	/* check if already subscribed to this topic */
+	MavlinkOrbSubscription *sub;
+
+	LL_FOREACH(_subscriptions, sub) {
+		if (sub->get_topic() == topic) {
+			/* already subscribed */
+			return sub;
+		}
+	}
+
+	/* add new subscription */
+	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic);
+
+	LL_APPEND(_subscriptions, sub_new);
+
+	return sub_new;
+}
+
+int
+Mavlink::configure_stream(const char *stream_name, const float rate)
+{
+	/* calculate interval in us, 0 means disabled stream */
+	unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0;
+
+	/* search if stream exists */
+	MavlinkStream *stream;
+	LL_FOREACH(_streams, stream) {
+		if (strcmp(stream_name, stream->get_name()) == 0) {
+			if (interval > 0) {
+				/* set new interval */
+				stream->set_interval(interval);
+
+			} else {
+				/* delete stream */
+				LL_DELETE(_streams, stream);
+				delete stream;
+				warnx("deleted stream %s", stream->get_name());
+			}
+
+			return OK;
+		}
+	}
+
+	if (interval == 0) {
+		/* stream was not active and is requested to be disabled, do nothing */
+		return OK;
+	}
+
+	/* search for stream with specified name in supported streams list */
+	for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+
+		if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
+			/* create new instance */
+			stream = streams_list[i]->new_instance();
+			stream->set_channel(get_channel());
+			stream->set_interval(interval);
+			stream->subscribe(this);
+			LL_APPEND(_streams, stream);
+
+			return OK;
+		}
+	}
+
+	/* if we reach here, the stream list does not contain the stream */
+	warnx("stream %s not found", stream_name);
+
+	return ERROR;
+}
+
+void
+Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
+{
+	/* orb subscription must be done from the main thread,
+	 * set _subscribe_to_stream and _subscribe_to_stream_rate fields
+	 * which polled in mavlink main loop */
+	if (!_task_should_exit) {
+		/* wait for previous subscription completion */
+		while (_subscribe_to_stream != nullptr) {
+			usleep(MAIN_LOOP_DELAY / 2);
+		}
+
+		/* copy stream name */
+		unsigned n = strlen(stream_name) + 1;
+		char *s = new char[n];
+		strcpy(s, stream_name);
+
+		/* set subscription task */
+		_subscribe_to_stream_rate = rate;
+		_subscribe_to_stream = s;
+
+		/* wait for subscription */
+		do {
+			usleep(MAIN_LOOP_DELAY / 2);
+		} while (_subscribe_to_stream != nullptr);
+	}
+}
+
+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);
+
+	int ret;
+	if (_message_buffer.data == 0) {
+		ret = ERROR;
+		_message_buffer.size = 0;
+	} else {
+		ret = OK;
+	}
+
+	return ret;
+}
+
+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(const 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(const 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);
+	}
+}
+
+float
+Mavlink::get_rate_mult()
+{
+	return _datarate / 1000.0f;
+}
+
+int
+Mavlink::task_main(int argc, char *argv[])
+{
+	int ch;
+	_baudrate = 57600;
+	_datarate = 0;
+	_mode = MAVLINK_MODE_NORMAL;
+
+	/* work around some stupidity in task_create's argv handling */
+	argc -= 2;
+	argv += 2;
+
+	/* don't exit from getopt loop to leave getopt global variables in consistent state,
+	 * set error flag instead */
+	bool err_flag = false;
+
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
+		switch (ch) {
+		case 'b':
+			_baudrate = strtoul(optarg, NULL, 10);
+
+			if (_baudrate < 9600 || _baudrate > 921600) {
+				warnx("invalid baud rate '%s'", optarg);
+				err_flag = true;
+			}
+
+			break;
+
+		case 'r':
+			_datarate = strtoul(optarg, NULL, 10);
+
+			if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
+				warnx("invalid data rate '%s'", optarg);
+				err_flag = true;
+			}
+
+			break;
+
+		case 'd':
+			_device_name = optarg;
+			break;
+
+//		case 'e':
+//			mavlink_link_termination_allowed = true;
+//			break;
+
+		case 'm':
+			if (strcmp(optarg, "custom") == 0) {
+				_mode = MAVLINK_MODE_CUSTOM;
+
+			} else if (strcmp(optarg, "camera") == 0) {
+				_mode = MAVLINK_MODE_CAMERA;
+			}
+
+			break;
+
+		case 'f':
+			_forwarding_on = true;
+			break;
+
+		case 'p':
+			_passing_on = true;
+			break;
+
+		case 'v':
+			_verbose = true;
+			break;
+
+		case 'w':
+			_wait_to_transmit = true;
+			break;
+
+		case 'x':
+			_ftp_on = true;
+			break;
+
+		default:
+			err_flag = true;
+			break;
+		}
+	}
+
+	if (err_flag) {
+		usage();
+		return ERROR;
+	}
+
+	if (_datarate == 0) {
+		/* convert bits to bytes and use 1/2 of bandwidth by default */
+		_datarate = _baudrate / 20;
+	}
+
+	if (_datarate > MAX_DATA_RATE) {
+		_datarate = MAX_DATA_RATE;
+	}
+
+	if (Mavlink::instance_exists(_device_name, this)) {
+		warnx("mavlink instance for %s already running", _device_name);
+		return ERROR;
+	}
+
+	/* inform about mode */
+	switch (_mode) {
+	case MAVLINK_MODE_NORMAL:
+		warnx("mode: NORMAL");
+		break;
+
+	case MAVLINK_MODE_CUSTOM:
+		warnx("mode: CUSTOM");
+		break;
+
+	case MAVLINK_MODE_CAMERA:
+		warnx("mode: CAMERA");
+		break;
+
+	default:
+		warnx("ERROR: Unknown mode");
+		break;
+	}
+
+	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
+
+	/* flush stdout in case MAVLink is about to take it over */
+	fflush(stdout);
+
+	struct termios uart_config_original;
+
+	/* default values for arguments */
+	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart);
+
+	if (_uart_fd < 0) {
+		warn("could not open %s", _device_name);
+		return ERROR;
+	}
+
+	/* 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 || _ftp_on) {
+		/* initialize message buffer if multiplexing is on or its needed for FTP.
+		 * make space for two messages plus off-by-one space as we use the empty element
+		 * marker ring buffer approach.
+		 */
+		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
+			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);
+
+	/* initialize logging device */
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	/* Initialize system properties */
+	mavlink_update_system();
+
+	/* start the MAVLink receiver */
+	_receive_thread = MavlinkReceiver::receive_start(this);
+
+	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+
+	/* create mission manager */
+	_mission_manager = new MavlinkMissionManager(this);
+	_mission_manager->set_verbose(_verbose);
+
+	_task_running = true;
+
+	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
+	uint64_t param_time = 0;
+	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
+	uint64_t status_time = 0;
+
+	struct vehicle_status_s status;
+	status_sub->update(&status_time, &status);
+
+	MavlinkCommandsStream commands_stream(this, _channel);
+
+	/* add default streams depending on mode and intervals depending on datarate */
+	float rate_mult = get_rate_mult();
+
+	configure_stream("HEARTBEAT", 1.0f);
+
+	switch (_mode) {
+	case MAVLINK_MODE_NORMAL:
+		configure_stream("SYS_STATUS", 1.0f);
+		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
+		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
+		configure_stream("ATTITUDE", 10.0f * rate_mult);
+		configure_stream("VFR_HUD", 10.0f * rate_mult);
+		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
+		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
+		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
+		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
+		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+		configure_stream("DISTANCE_SENSOR", 0.5f);
+		break;
+
+	case MAVLINK_MODE_CAMERA:
+		configure_stream("SYS_STATUS", 1.0f);
+		configure_stream("ATTITUDE", 15.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
+		configure_stream("CAMERA_CAPTURE", 1.0f);
+		break;
+
+	default:
+		break;
+	}
+
+	/* don't send parameters on startup without request */
+	_mavlink_param_queue_index = param_count();
+
+	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
+
+	/* set main loop delay depending on data rate to minimize CPU overhead */
+	_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
+
+	/* now the instance is fully initialized and we can bump the instance count */
+	LL_APPEND(_mavlink_instances, this);
+
+	while (!_task_should_exit) {
+		/* main loop */
+		usleep(_main_loop_delay);
+
+		perf_begin(_loop_perf);
+
+		hrt_abstime t = hrt_absolute_time();
+
+		if (param_sub->update(&param_time, nullptr)) {
+			/* parameters updated */
+			mavlink_update_system();
+		}
+
+		if (status_sub->update(&status_time, &status)) {
+			/* switch HIL mode if required */
+			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)) {
+				if (_subscribe_to_stream_rate > 0.0f) {
+					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
+
+				} else {
+					warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
+				}
+
+			} else {
+				warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
+			}
+
+			delete _subscribe_to_stream;
+			_subscribe_to_stream = nullptr;
+		}
+
+		/* update streams */
+		MavlinkStream *stream;
+		LL_FOREACH(_streams, stream) {
+			stream->update(t);
+		}
+
+		if (fast_rate_limiter.check(t)) {
+			mavlink_pm_queued_send();
+			_mission_manager->eventloop();
+
+			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
+				struct mavlink_logmessage msg;
+				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
+
+				if (lb_ret == OK) {
+					send_statustext(msg.severity, msg.text);
+				}
+			}
+		}
+
+		/* pass messages from other UARTs or FTP worker */
+		if (_passing_on || _ftp_on) {
+
+			bool is_part;
+			uint8_t *read_ptr;
+            uint8_t *write_ptr;
+
+			pthread_mutex_lock(&_message_buffer_mutex);
+			int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+			pthread_mutex_unlock(&_message_buffer_mutex);
+
+			if (available > 0) {
+                // Reconstruct message from buffer
+
+				mavlink_message_t msg;
+                write_ptr = (uint8_t*)&msg;
+
+                // Pull a single message from the buffer
+                size_t read_count = available;
+                if (read_count > sizeof(mavlink_message_t)) {
+                    read_count = sizeof(mavlink_message_t);
+                }
+                
+                memcpy(write_ptr, read_ptr, read_count);
+                
+                // We hold the mutex until after we complete the second part of the buffer. If we don't
+                // we may end up breaking the empty slot overflow detection semantics when we mark the
+                // possibly partial read below.
+                pthread_mutex_lock(&_message_buffer_mutex);
+                
+				message_buffer_mark_read(read_count);
+
+				/* write second part of buffer if there is some */
+				if (is_part && read_count < sizeof(mavlink_message_t)) {
+                    write_ptr += read_count;
+					available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+                    read_count = sizeof(mavlink_message_t) - read_count;
+                    memcpy(write_ptr, read_ptr, read_count);
+					message_buffer_mark_read(available);
+				}
+                
+                pthread_mutex_unlock(&_message_buffer_mutex);
+
+                _mavlink_resend_uart(_channel, &msg);
+			}
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	delete _mission_manager;
+
+	delete _subscribe_to_stream;
+	_subscribe_to_stream = nullptr;
+
+	/* delete streams */
+	MavlinkStream *stream_to_del = nullptr;
+	MavlinkStream *stream_next = _streams;
+
+	while (stream_next != nullptr) {
+		stream_to_del = stream_next;
+		stream_next = stream_to_del->next;
+		delete stream_to_del;
+	}
+
+	_streams = nullptr;
+
+	/* delete subscriptions */
+	MavlinkOrbSubscription *sub_to_del = nullptr;
+	MavlinkOrbSubscription *sub_next = _subscriptions;
+
+	while (sub_next != nullptr) {
+		sub_to_del = sub_next;
+		sub_next = sub_to_del->next;
+		delete sub_to_del;
+	}
+
+	_subscriptions = nullptr;
+
+	warnx("waiting for UART receive thread");
+
+	/* wait for threads to complete */
+	pthread_join(_receive_thread, NULL);
+
+	/* reset the UART flags to original state */
+	tcsetattr(_uart_fd, TCSANOW, &uart_config_original);
+
+	/* close UART */
+	close(_uart_fd);
+
+	/* close mavlink logging device */
+	close(_mavlink_fd);
+
+	if (_passing_on || _ftp_on) {
+		message_buffer_destroy();
+		pthread_mutex_destroy(&_message_buffer_mutex);
+	}
+	/* destroy log buffer */
+	mavlink_logbuffer_destroy(&_logbuffer);
+
+	warnx("exiting");
+	_task_running = false;
+
+	return OK;
+}
+
+int Mavlink::start_helper(int argc, char *argv[])
+{
+	/* create the instance in task context */
+	Mavlink *instance = new Mavlink();
+
+	int res;
+
+	if (!instance) {
+
+		/* out of memory */
+		res = -ENOMEM;
+		warnx("OUT OF MEM");
+	} else {
+		/* this will actually only return once MAVLink exits */
+		res = instance->task_main(argc, argv);
+
+		/* delete instance on main thread end */
+		delete instance;
+	}
+
+	return res;
+}
+
+int
+Mavlink::start(int argc, char *argv[])
+{
+	// Wait for the instance count to go up one
+	// before returning to the shell
+	int ic = Mavlink::instance_count();
+
+	// Instantiate thread
+	char buf[24];
+	sprintf(buf, "mavlink_if%d", ic);
+
+	// This is where the control flow splits
+	// between the starting task and the spawned
+	// task - start_helper() only returns
+	// when the started task exits.
+	task_spawn_cmd(buf,
+		       SCHED_DEFAULT,
+		       SCHED_PRIORITY_DEFAULT,
+		       2700,
+		       (main_t)&Mavlink::start_helper,
+		       (const char **)argv);
+
+	// Ensure that this shell command
+	// does not return before the instance
+	// is fully initialized. As this is also
+	// the only path to create a new instance,
+	// this is effectively a lock on concurrent
+	// instance starting. XXX do a real lock.
+
+	// Sleep 500 us between each attempt
+	const unsigned sleeptime = 500;
+
+	// Wait 100 ms max for the startup.
+	const unsigned limit = 100 * 1000 / sleeptime;
+
+	unsigned count = 0;
+
+	while (ic == Mavlink::instance_count() && count < limit) {
+		::usleep(sleeptime);
+		count++;
+	}
+
+	return OK;
+}
+
+void
+Mavlink::display_status()
+{
+	warnx("running");
+}
+
+int
+Mavlink::stream_command(int argc, char *argv[])
+{
+	const char *device_name = DEFAULT_DEVICE_NAME;
+	float rate = -1.0f;
+	const char *stream_name = nullptr;
+
+	argc -= 2;
+	argv += 2;
+
+	/* don't exit from getopt loop to leave getopt global variables in consistent state,
+	 * set error flag instead */
+	bool err_flag = false;
+
+	int i = 0;
+
+	while (i < argc) {
+
+		if (0 == strcmp(argv[i], "-r") && i < argc - 1) {
+			rate = strtod(argv[i + 1], nullptr);
+
+			if (rate < 0.0f) {
+				err_flag = true;
+			}
+
+			i++;
+
+		} else if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
+			device_name = argv[i + 1];
+			i++;
+
+		} else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
+			stream_name = argv[i + 1];
+			i++;
+
+		} else {
+			err_flag = true;
+		}
+
+		i++;
+	}
+
+	if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
+		Mavlink *inst = get_instance_for_device(device_name);
+
+		if (inst != nullptr) {
+			inst->configure_stream_threadsafe(stream_name, rate);
+
+		} else {
+
+			// If the link is not running we should complain, but not fall over
+			// because this is so easy to get wrong and not fatal. Warning is sufficient.
+			errx(0, "mavlink for device %s is not running", device_name);
+		}
+
+	} else {
+		errx(1, "usage: mavlink stream [-d device] -s stream -r rate");
+	}
+
+	return OK;
+}
+
+static void usage()
+{
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
+}
+
+int mavlink_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage();
+		exit(1);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		return Mavlink::start(argc, argv);
+
+	} else if (!strcmp(argv[1], "stop")) {
+		warnx("mavlink stop is deprecated, use stop-all instead");
+		usage();
+		exit(1);
+
+	} else if (!strcmp(argv[1], "stop-all")) {
+		return Mavlink::destroy_all_instances();
+
+		// } else if (!strcmp(argv[1], "status")) {
+		// 	mavlink::g_mavlink->status();
+
+	} else if (!strcmp(argv[1], "stream")) {
+		return Mavlink::stream_command(argc, argv);
+
+	} else {
+		usage();
+		exit(1);
+	}
+
+	return 0;
+}
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
new file mode 100644
index 000000000..acfc8b90e
--- /dev/null
+++ b/src/modules/mavlink/mavlink_main.h
@@ -0,0 +1,375 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_main.h
+ * MAVLink 1.0 protocol interface definition.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <nuttx/fs/fs.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <pthread.h>
+#include <mavlink/mavlink_log.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+#include "mavlink_bridge_header.h"
+#include "mavlink_orb_subscription.h"
+#include "mavlink_stream.h"
+#include "mavlink_messages.h"
+#include "mavlink_mission.h"
+
+
+class Mavlink
+{
+
+public:
+	/**
+	 * Constructor
+	 */
+	Mavlink();
+
+	/**
+	 * Destructor, also kills the mavlinks task.
+	 */
+	~Mavlink();
+
+	/**
+	* Start the mavlink task.
+	 *
+	 * @return		OK on success.
+	 */
+	static int		start(int argc, char *argv[]);
+
+	/**
+	 * Display the mavlink status.
+	 */
+	void			display_status();
+
+	static int		stream_command(int argc, char *argv[]);
+
+	static int		instance_count();
+
+	static Mavlink		*new_instance();
+
+	static Mavlink		*get_instance(unsigned instance);
+
+	static Mavlink		*get_instance_for_device(const char *device_name);
+
+	static int		destroy_all_instances();
+
+	static bool		instance_exists(const char *device_name, Mavlink *self);
+
+	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
+
+	static int		get_uart_fd(unsigned index);
+
+	int			get_uart_fd();
+
+	/**
+	 * Get the MAVLink system id.
+	 *
+	 * @return		The system ID of this vehicle
+	 */
+	int			get_system_id();
+
+	/**
+	 * Get the MAVLink component id.
+	 *
+	 * @return		The component ID of this vehicle
+	 */
+	int			get_component_id();
+
+	const char *_device_name;
+
+	enum MAVLINK_MODE {
+		MAVLINK_MODE_NORMAL = 0,
+		MAVLINK_MODE_CUSTOM,
+		MAVLINK_MODE_CAMERA
+	};
+
+	void			set_mode(enum MAVLINK_MODE);
+	enum MAVLINK_MODE	get_mode() { return _mode; }
+
+	bool			get_hil_enabled() { return _hil_enabled; }
+
+	bool			get_use_hil_gps() { return _use_hil_gps; }
+
+	bool			get_flow_control_enabled() { return _flow_control_enabled; }
+
+	bool			get_forwarding_on() { return _forwarding_on; }
+
+	static int		start_helper(int argc, char *argv[]);
+
+	/**
+	 * Handle parameter related messages.
+	 */
+	void			mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+
+	void			get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+
+	/**
+	 * Enable / disable Hardware in the Loop simulation mode.
+	 *
+	 * @param hil_enabled	The new HIL enable/disable state.
+	 * @return		OK if the HIL state changed, ERROR if the
+	 *			requested change could not be made or was
+	 *			redundant.
+	 */
+	int			set_hil_enabled(bool hil_enabled);
+
+	void			send_message(const mavlink_message_t *msg);
+
+	void			handle_message(const mavlink_message_t *msg);
+
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+
+	int			get_instance_id();
+
+	/**
+	 * Enable / disable hardware flow control.
+	 *
+	 * @param enabled	True if hardware flow control should be enabled
+	 */
+	int			enable_flow_control(bool enabled);
+
+	mavlink_channel_t	get_channel();
+
+	void			configure_stream_threadsafe(const char *stream_name, float rate);
+
+	bool			_task_should_exit;	/**< if true, mavlink task should exit */
+
+	int			get_mavlink_fd() { return _mavlink_fd; }
+
+	/**
+	 * Send a status text with loglevel INFO
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_info(const char *string);
+
+	/**
+	 * Send a status text with loglevel CRITICAL
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_critical(const char *string);
+
+	/**
+	 * Send a status text with loglevel EMERGENCY
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_emergency(const char *string);
+
+	/**
+	 * Send a status text with loglevel
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 * @param severity the log level, one of 
+	 */
+	int			send_statustext(unsigned severity, const char *string);
+	MavlinkStream *		get_streams() const { return _streams; }
+
+	float			get_rate_mult();
+
+	/* 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)); }
+
+	bool			message_buffer_write(const void *ptr, int size);
+    
+	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
+
+	/**
+	 * Count a transmision error
+	 */
+	void			count_txerr();
+
+protected:
+	Mavlink			*next;
+
+private:
+	int			_instance_id;
+
+	int			_mavlink_fd;
+	bool			_task_running;
+
+	/* states */
+	bool			_hil_enabled;		/**< Hardware In the Loop mode */
+	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
+	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 */
+
+	MavlinkOrbSubscription	*_subscriptions;
+	MavlinkStream		*_streams;
+
+	MavlinkMissionManager *_mission_manager;
+
+	orb_advert_t	_mission_pub;
+	int			_mission_result_sub;
+	MAVLINK_MODE _mode;
+
+	mavlink_channel_t _channel;
+
+	struct mavlink_logbuffer _logbuffer;
+	unsigned int		_total_counter;
+
+	pthread_t		_receive_thread;
+
+	bool			_verbose;
+	bool			_forwarding_on;
+	bool			_passing_on;
+	bool			_ftp_on;
+	int			_uart_fd;
+	int			_baudrate;
+	int			_datarate;
+
+	/**
+	 * If the queue index is not at 0, the queue sending
+	 * logic will send parameters from the current index
+	 * to len - 1, the end of the param list.
+	 */
+	unsigned int		_mavlink_param_queue_index;
+
+	bool			mavlink_link_termination_allowed;
+
+	char 			*_subscribe_to_stream;
+	float			_subscribe_to_stream_rate;
+
+	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;
+
+	bool			_param_initialized;
+	param_t			_param_system_id;
+	param_t			_param_component_id;
+	param_t			_param_system_type;
+	param_t			_param_use_hil_gps;
+
+	perf_counter_t		_loop_perf;			/**< loop performance counter */
+	perf_counter_t		_txerr_perf;			/**< TX error counter */
+
+	/**
+	 * Send one parameter.
+	 *
+	 * @param param		The parameter id to send.
+	 * @return		zero on success, nonzero on failure.
+	 */
+	int			mavlink_pm_send_param(param_t param);
+
+	/**
+	 * Send one parameter identified by index.
+	 *
+	 * @param index		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	int			mavlink_pm_send_param_for_index(uint16_t index);
+
+	/**
+	 * Send one parameter identified by name.
+	 *
+	 * @param name		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	int			mavlink_pm_send_param_for_name(const char *name);
+
+	/**
+	 * Send a queue of parameters, one parameter per function call.
+	 *
+	 * @return		zero on success, nonzero on failure
+	 */
+	int			mavlink_pm_queued_send(void);
+
+	/**
+	 * Start sending the parameter queue.
+	 *
+	 * This function will not directly send parameters, but instead
+	 * activate the sending of one parameter on each call of
+	 * mavlink_pm_queued_send().
+	 * @see 		mavlink_pm_queued_send()
+	 */
+	void			mavlink_pm_start_queued_send();
+
+	void			mavlink_update_system();
+
+	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
+
+	int configure_stream(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();
+
+	int message_buffer_get_ptr(void **ptr, bool *is_part);
+
+	void message_buffer_mark_read(int n);
+
+	void pass_message(const mavlink_message_t *msg);
+
+	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
+
+	/**
+	 * Main mavlink task.
+	 */
+	int		task_main(int argc, char *argv[]);
+};
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
new file mode 100644
index 000000000..7c864f127
--- /dev/null
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -0,0 +1,1796 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_messages.cpp
+ * MAVLink 1.0 message formatters implementation.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <stdio.h>
+#include <commander/px4_custom_mode.h>
+#include <lib/geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/navigation_capabilities.h>
+#include <drivers/drv_rc_input.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_range_finder.h>
+
+#include <systemlib/err.h>
+
+#include "mavlink_messages.h"
+
+
+static uint16_t cm_uint16_from_m_float(float m);
+static 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);
+
+uint16_t
+cm_uint16_from_m_float(float m)
+{
+	if (m < 0.0f) {
+		return 0;
+
+	} else if (m > 655.35f) {
+		return 65535;
+	}
+
+	return (uint16_t)(m * 100.0f);
+}
+
+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)
+{
+	*mavlink_state = 0;
+	*mavlink_base_mode = 0;
+	*mavlink_custom_mode = 0;
+
+	/* HIL */
+	if (status->hil_state == HIL_STATE_ON) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	}
+
+	/* arming state */
+	if (status->arming_state == ARMING_STATE_ARMED
+	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+	}
+
+	/* main state */
+	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
+
+	switch (status->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;
+
+		case NAVIGATION_STATE_ACRO:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
+			break;
+
+		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;
+
+		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;
+
+		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;
+
+		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:
+			*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;
+
+		case NAVIGATION_STATE_LAND:
+			/* fallthrough */
+		case NAVIGATION_STATE_DESCEND:
+			*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_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;
+			break;
+
+		case NAVIGATION_STATE_OFFBOARD:
+			*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_OFFBOARD;
+			break;
+
+		case NAVIGATION_STATE_MAX:
+			/* this is an unused case, ignore */
+			break;
+
+	}
+
+	*mavlink_custom_mode = custom_mode.data;
+
+	/* set system state */
+	if (status->arming_state == ARMING_STATE_INIT
+	    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
+	    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		*mavlink_state = MAV_STATE_UNINIT;
+
+	} else if (status->arming_state == ARMING_STATE_ARMED) {
+		*mavlink_state = MAV_STATE_ACTIVE;
+
+	} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_state = MAV_STATE_CRITICAL;
+
+	} else if (status->arming_state == ARMING_STATE_STANDBY) {
+		*mavlink_state = MAV_STATE_STANDBY;
+
+	} else if (status->arming_state == ARMING_STATE_REBOOT) {
+		*mavlink_state = MAV_STATE_POWEROFF;
+
+	} else {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	}
+}
+
+
+class MavlinkStreamHeartbeat : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamHeartbeat::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "HEARTBEAT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HEARTBEAT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHeartbeat();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+	MavlinkOrbSubscription *pos_sp_triplet_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_status_s status;
+		struct position_setpoint_triplet_s pos_sp_triplet;
+
+		/* always send the heartbeat, independent of the update status of the topics */
+		if (!status_sub->update(&status)) {
+			/* if topic update failed fill it with defaults */
+			memset(&status, 0, sizeof(status));
+		}
+
+		if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+			/* if topic update failed fill it with defaults */
+			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+		}
+
+		uint8_t mavlink_state = 0;
+		uint8_t mavlink_base_mode = 0;
+		uint32_t mavlink_custom_mode = 0;
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+		mavlink_msg_heartbeat_send(_channel,
+					   mavlink_system.type,
+					   MAV_AUTOPILOT_PX4,
+					   mavlink_base_mode,
+					   mavlink_custom_mode,
+					   mavlink_state);
+	}
+};
+
+
+class MavlinkStreamSysStatus : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamSysStatus::get_name_static();
+	}
+
+	static const char *get_name_static ()
+	{
+		return "SYS_STATUS";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SYS_STATUS;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamSysStatus();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_status_s status;
+
+		if (status_sub->update(&status)) {
+			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 * 100.0f,
+							status.battery_remaining * 100.0f,
+							status.drop_rate_comm,
+							status.errors_comm,
+							status.errors_count1,
+							status.errors_count2,
+							status.errors_count3,
+							status.errors_count4);
+		}
+	}
+};
+
+
+class MavlinkStreamHighresIMU : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamHighresIMU::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "HIGHRES_IMU";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIGHRES_IMU;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHighresIMU();
+	}
+
+private:
+	MavlinkOrbSubscription *sensor_sub;
+	uint64_t sensor_time;
+
+	uint64_t accel_timestamp;
+	uint64_t gyro_timestamp;
+	uint64_t mag_timestamp;
+	uint64_t baro_timestamp;
+
+protected:
+	explicit MavlinkStreamHighresIMU() : MavlinkStream(),
+		sensor_time(0),
+		accel_timestamp(0),
+		gyro_timestamp(0),
+		mag_timestamp(0),
+		baro_timestamp(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct sensor_combined_s sensor;
+
+		if (sensor_sub->update(&sensor_time, &sensor)) {
+			uint16_t fields_updated = 0;
+
+			if (accel_timestamp != sensor.accelerometer_timestamp) {
+				/* mark first three dimensions as changed */
+				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
+				accel_timestamp = sensor.accelerometer_timestamp;
+			}
+
+			if (gyro_timestamp != sensor.timestamp) {
+				/* mark second group dimensions as changed */
+				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
+				gyro_timestamp = sensor.timestamp;
+			}
+
+			if (mag_timestamp != sensor.magnetometer_timestamp) {
+				/* mark third group dimensions as changed */
+				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
+				mag_timestamp = sensor.magnetometer_timestamp;
+			}
+
+			if (baro_timestamp != sensor.baro_timestamp) {
+				/* mark last group dimensions as changed */
+				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
+				baro_timestamp = sensor.baro_timestamp;
+			}
+
+			mavlink_msg_highres_imu_send(_channel,
+						     sensor.timestamp,
+						     sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2],
+						     sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2],
+						     sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2],
+						     sensor.baro_pres_mbar, sensor.differential_pressure_pa,
+						     sensor.baro_alt_meter, sensor.baro_temp_celcius,
+						     fields_updated);
+		}
+	}
+};
+
+
+class MavlinkStreamAttitude : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitude::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ATTITUDE";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitude();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sub;
+	uint64_t att_time;
+
+protected:
+	explicit MavlinkStreamAttitude() : MavlinkStream(),
+		att_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_attitude_s att;
+
+		if (att_sub->update(&att_time, &att)) {
+			mavlink_msg_attitude_send(_channel,
+						  att.timestamp / 1000,
+						  att.roll, att.pitch, att.yaw,
+						  att.rollspeed, att.pitchspeed, att.yawspeed);
+		}
+	}
+};
+
+
+class MavlinkStreamAttitudeQuaternion : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitudeQuaternion::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ATTITUDE_QUATERNION";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitudeQuaternion();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sub;
+	uint64_t att_time;
+
+protected:
+	explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
+		att_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_attitude_s att;
+
+		if (att_sub->update(&att_time, &att)) {
+			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);
+		}
+	}
+};
+
+
+class MavlinkStreamVFRHUD : public MavlinkStream
+{
+public:
+
+	const char *get_name() const
+	{
+		return MavlinkStreamVFRHUD::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "VFR_HUD";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VFR_HUD;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamVFRHUD();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sub;
+	uint64_t att_time;
+
+	MavlinkOrbSubscription *pos_sub;
+	uint64_t pos_time;
+
+	MavlinkOrbSubscription *armed_sub;
+	uint64_t armed_time;
+
+	MavlinkOrbSubscription *act_sub;
+	uint64_t act_time;
+
+	MavlinkOrbSubscription *airspeed_sub;
+	uint64_t airspeed_time;
+
+protected:
+	explicit MavlinkStreamVFRHUD() : MavlinkStream(),
+		att_time(0),
+		pos_time(0),
+		armed_time(0),
+		act_time(0),
+		airspeed_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
+		armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
+		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
+		airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_attitude_s att;
+		struct vehicle_global_position_s pos;
+		struct actuator_armed_s armed;
+		struct actuator_controls_s act;
+		struct airspeed_s airspeed;
+
+		bool updated = att_sub->update(&att_time, &att);
+		updated |= pos_sub->update(&pos_time, &pos);
+		updated |= armed_sub->update(&armed_time, &armed);
+		updated |= act_sub->update(&act_time, &act);
+		updated |= airspeed_sub->update(&airspeed_time, &airspeed);
+
+		if (updated) {
+			float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
+			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+			float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
+
+			mavlink_msg_vfr_hud_send(_channel,
+						 airspeed.true_airspeed_m_s,
+						 groundspeed,
+						 heading,
+						 throttle,
+						 pos.alt,
+						 -pos.vel_d);
+		}
+	}
+};
+
+
+class MavlinkStreamGPSRawInt : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamGPSRawInt::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GPS_RAW_INT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_RAW_INT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGPSRawInt();
+	}
+
+private:
+	MavlinkOrbSubscription *gps_sub;
+	uint64_t gps_time;
+
+protected:
+	explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
+		gps_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_gps_position_s gps;
+
+		if (gps_sub->update(&gps_time, &gps)) {
+			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),
+						     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_used);
+		}
+	}
+};
+
+
+class MavlinkStreamGlobalPositionInt : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamGlobalPositionInt::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GLOBAL_POSITION_INT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGlobalPositionInt();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sub;
+	uint64_t pos_time;
+
+	MavlinkOrbSubscription *home_sub;
+	uint64_t home_time;
+
+protected:
+	explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
+		pos_time(0),
+		home_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
+		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_global_position_s pos;
+		struct home_position_s home;
+
+		bool updated = pos_sub->update(&pos_time, &pos);
+		updated |= home_sub->update(&home_time, &home);
+
+		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);
+		}
+	}
+};
+
+
+class MavlinkStreamLocalPositionNED : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamLocalPositionNED::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "LOCAL_POSITION_NED";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamLocalPositionNED();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sub;
+	uint64_t pos_time;
+
+protected:
+	explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
+		pos_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_local_position_s pos;
+
+		if (pos_sub->update(&pos_time, &pos)) {
+			mavlink_msg_local_position_ned_send(_channel,
+							    pos.timestamp / 1000,
+							    pos.x,
+							    pos.y,
+							    pos.z,
+							    pos.vx,
+							    pos.vy,
+							    pos.vz);
+		}
+	}
+};
+
+
+
+class MavlinkStreamViconPositionEstimate : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamViconPositionEstimate::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "VICON_POSITION_ESTIMATE";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamViconPositionEstimate();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sub;
+	uint64_t pos_time;
+
+protected:
+	explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
+		pos_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_vicon_position_s pos;
+
+		if (pos_sub->update(&pos_time, &pos)) {
+			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:
+	const char *get_name() const
+	{
+		return MavlinkStreamGPSGlobalOrigin::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GPS_GLOBAL_ORIGIN";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGPSGlobalOrigin();
+	}
+
+private:
+	MavlinkOrbSubscription *home_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		/* we're sending the GPS home periodically to ensure the
+		 * the GCS does pick it up at one point */
+		if (home_sub->is_published()) {
+			struct home_position_s home;
+
+			if (home_sub->update(&home)) {
+				mavlink_msg_gps_global_origin_send(_channel,
+								   (int32_t)(home.lat * 1e7),
+								   (int32_t)(home.lon * 1e7),
+								   (int32_t)(home.alt) * 1000.0f);
+			}
+		}
+	}
+};
+
+template <int N>
+class MavlinkStreamServoOutputRaw : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamServoOutputRaw<N>::get_name_static();
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
+	}
+
+	static const char *get_name_static()
+	{
+		switch (N) {
+			case 0:
+			return "SERVO_OUTPUT_RAW_0";
+
+			case 1:
+			return "SERVO_OUTPUT_RAW_1";
+
+			case 2:
+			return "SERVO_OUTPUT_RAW_2";
+
+			case 3:
+			return "SERVO_OUTPUT_RAW_3";
+		}
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamServoOutputRaw<N>();
+	}
+
+private:
+	MavlinkOrbSubscription *act_sub;
+	uint64_t act_time;
+
+protected:
+	explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
+		act_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		orb_id_t act_topics[] = {
+			ORB_ID(actuator_outputs_0),
+			ORB_ID(actuator_outputs_1),
+			ORB_ID(actuator_outputs_2),
+			ORB_ID(actuator_outputs_3)
+		};
+
+		act_sub = mavlink->add_orb_subscription(act_topics[N]);
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct actuator_outputs_s act;
+
+		if (act_sub->update(&act_time, &act)) {
+			mavlink_msg_servo_output_raw_send(_channel,
+							  act.timestamp / 1000,
+							  N,
+							  act.output[0],
+							  act.output[1],
+							  act.output[2],
+							  act.output[3],
+							  act.output[4],
+							  act.output[5],
+							  act.output[6],
+							  act.output[7]);
+		}
+	}
+};
+
+
+class MavlinkStreamHILControls : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamHILControls::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "HIL_CONTROLS";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIL_CONTROLS;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHILControls();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+	uint64_t status_time;
+
+	MavlinkOrbSubscription *pos_sp_triplet_sub;
+	uint64_t pos_sp_triplet_time;
+
+	MavlinkOrbSubscription *act_sub;
+	uint64_t act_time;
+
+protected:
+	explicit MavlinkStreamHILControls() : MavlinkStream(),
+		status_time(0),
+		pos_sp_triplet_time(0),
+		act_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
+		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_status_s status;
+		struct position_setpoint_triplet_s pos_sp_triplet;
+		struct actuator_outputs_s act;
+
+		bool updated = act_sub->update(&act_time, &act);
+		updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
+		updated |= status_sub->update(&status_time, &status);
+
+		if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
+			/* translate the current syste state to mavlink state and mode */
+			uint8_t mavlink_state;
+			uint8_t mavlink_base_mode;
+			uint32_t mavlink_custom_mode;
+			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+			float out[8];
+
+			const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+
+			/* scale outputs depending on system type */
+			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
+				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
+				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+				/* multirotors: set number of rotor outputs depending on type */
+
+				unsigned n;
+
+				switch (mavlink_system.type) {
+				case MAV_TYPE_QUADROTOR:
+					n = 4;
+					break;
+
+				case MAV_TYPE_HEXAROTOR:
+					n = 6;
+					break;
+
+				default:
+					n = 8;
+					break;
+				}
+
+				for (unsigned i = 0; i < 8; i++) {
+					if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+						if (i < n) {
+							/* scale PWM out 900..2100 us to 0..1 for rotors */
+							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+
+						} else {
+							/* scale PWM out 900..2100 us to -1..1 for other channels */
+							out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+						}
+
+					} else {
+						/* send 0 when disarmed */
+						out[i] = 0.0f;
+					}
+				}
+
+			} else {
+				/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
+
+				for (unsigned i = 0; i < 8; i++) {
+					if (i != 3) {
+						/* scale PWM out 900..2100 us to -1..1 for normal channels */
+						out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+
+					} else {
+						/* scale PWM out 900..2100 us to 0..1 for throttle */
+						out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+					}
+
+				}
+			}
+
+			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);
+		}
+	}
+};
+
+
+class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GLOBAL_POSITION_SETPOINT_INT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGlobalPositionSetpointInt();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sp_triplet_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct position_setpoint_triplet_s pos_sp_triplet;
+
+		if (pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+			mavlink_msg_global_position_setpoint_int_send(_channel,
+					MAV_FRAME_GLOBAL,
+					(int32_t)(pos_sp_triplet.current.lat * 1e7),
+					(int32_t)(pos_sp_triplet.current.lon * 1e7),
+					(int32_t)(pos_sp_triplet.current.alt * 1000),
+					(int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f));
+		}
+	}
+};
+
+
+class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamLocalPositionSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "LOCAL_POSITION_SETPOINT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamLocalPositionSetpoint();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sp_sub;
+	uint64_t pos_sp_time;
+
+protected:
+	explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
+		pos_sp_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_local_position_setpoint_s pos_sp;
+
+		if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
+			mavlink_msg_local_position_setpoint_send(_channel,
+					MAV_FRAME_LOCAL_NED,
+					pos_sp.x,
+					pos_sp.y,
+					pos_sp.z,
+					pos_sp.yaw);
+		}
+	}
+};
+
+
+class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamRollPitchYawThrustSetpoint();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sp_sub;
+	uint64_t att_sp_time;
+
+protected:
+	explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
+		att_sp_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_attitude_setpoint_s att_sp;
+
+		if (att_sp_sub->update(&att_sp_time, &att_sp)) {
+			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
+					att_sp.timestamp / 1000,
+					att_sp.roll_body,
+					att_sp.pitch_body,
+					att_sp.yaw_body,
+					att_sp.thrust);
+		}
+	}
+};
+
+
+class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
+	}
+
+private:
+	MavlinkOrbSubscription *att_rates_sp_sub;
+	uint64_t att_rates_sp_time;
+
+protected:
+	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
+		att_rates_sp_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_rates_setpoint_s att_rates_sp;
+
+		if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
+			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
+					att_rates_sp.timestamp / 1000,
+					att_rates_sp.roll,
+					att_rates_sp.pitch,
+					att_rates_sp.yaw,
+					att_rates_sp.thrust);
+		}
+	}
+};
+
+
+class MavlinkStreamRCChannelsRaw : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamRCChannelsRaw::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "RC_CHANNELS_RAW";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamRCChannelsRaw();
+	}
+
+private:
+	MavlinkOrbSubscription *rc_sub;
+	uint64_t rc_time;
+
+protected:
+	explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
+		rc_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct rc_input_values rc;
+
+		if (rc_sub->update(&rc_time, &rc)) {
+			const unsigned port_width = 8;
+
+			// Deprecated message (but still needed for compatibility!)
+			for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
+				/* Channels are sent in MAVLink main loop at a fixed interval */
+				mavlink_msg_rc_channels_raw_send(_channel,
+								 rc.timestamp_publication / 1000,
+								 i,
+								 (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
+								 rc.rssi);
+			}
+
+			// New message
+			mavlink_msg_rc_channels_send(_channel,
+					rc.timestamp_publication / 1000,
+					rc.channel_count,
+					((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
+					((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
+					((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
+					((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
+					((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
+					((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
+					((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
+					((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
+					((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
+					((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
+					((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
+					((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
+					((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
+					((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
+					((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
+					((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
+					((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
+					((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
+					rc.rssi);
+		}
+	}
+};
+
+
+class MavlinkStreamManualControl : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamManualControl::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "MANUAL_CONTROL";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MANUAL_CONTROL;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamManualControl();
+	}
+
+private:
+	MavlinkOrbSubscription *manual_sub;
+	uint64_t manual_time;
+
+protected:
+	explicit MavlinkStreamManualControl() : MavlinkStream(),
+		manual_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct manual_control_setpoint_s manual;
+
+		if (manual_sub->update(&manual_time, &manual)) {
+			mavlink_msg_manual_control_send(_channel,
+							mavlink_system.sysid,
+							manual.x * 1000,
+							manual.y * 1000,
+							manual.z * 1000,
+							manual.r * 1000,
+							0);
+		}
+	}
+};
+
+
+class MavlinkStreamOpticalFlow : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamOpticalFlow::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "OPTICAL_FLOW";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamOpticalFlow();
+	}
+
+private:
+	MavlinkOrbSubscription *flow_sub;
+	uint64_t flow_time;
+
+protected:
+	explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
+		flow_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct optical_flow_s flow;
+
+		if (flow_sub->update(&flow_time, &flow)) {
+			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);
+		}
+	}
+};
+
+class MavlinkStreamAttitudeControls : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitudeControls::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ATTITUDE_CONTROLS";
+	}
+
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitudeControls();
+	}
+
+private:
+	MavlinkOrbSubscription *att_ctrl_sub;
+	uint64_t att_ctrl_time;
+
+protected:
+	explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
+		att_ctrl_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct actuator_controls_s att_ctrl;
+
+		if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) {
+			/* send, add spaces so that string buffer is at least 10 chars long */
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl.timestamp / 1000,
+							   "rll ctrl    ",
+							   att_ctrl.control[0]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl.timestamp / 1000,
+							   "ptch ctrl    ",
+							   att_ctrl.control[1]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl.timestamp / 1000,
+							   "yaw ctrl     ",
+							   att_ctrl.control[2]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl.timestamp / 1000,
+							   "thr ctrl     ",
+							   att_ctrl.control[3]);
+		}
+	}
+};
+
+class MavlinkStreamNamedValueFloat : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamNamedValueFloat::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "NAMED_VALUE_FLOAT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamNamedValueFloat();
+	}
+
+private:
+	MavlinkOrbSubscription *debug_sub;
+	uint64_t debug_time;
+
+protected:
+	explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
+		debug_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct debug_key_value_s debug;
+
+		if (debug_sub->update(&debug_time, &debug)) {
+			/* enforce null termination */
+			debug.key[sizeof(debug.key) - 1] = '\0';
+
+			mavlink_msg_named_value_float_send(_channel,
+							   debug.timestamp_ms,
+							   debug.key,
+							   debug.value);
+		}
+	}
+};
+
+class MavlinkStreamCameraCapture : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamCameraCapture::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "CAMERA_CAPTURE";
+	}
+
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamCameraCapture();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_status_s status;
+		(void)status_sub->update(&status);
+
+		if (status.arming_state == ARMING_STATE_ARMED
+		    || 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);
+
+		} else {
+			/* send camera capture off */
+			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+		}
+	}
+};
+
+class MavlinkStreamDistanceSensor : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamDistanceSensor::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "DISTANCE_SENSOR";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamDistanceSensor();
+	}
+
+private:
+	MavlinkOrbSubscription *range_sub;
+	uint64_t range_time;
+
+protected:
+	explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
+		range_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct range_finder_report range;
+
+		if (range_sub->update(&range_time, &range)) {
+
+			uint8_t type;
+
+			switch (range.type) {
+				case RANGE_FINDER_TYPE_LASER:
+				type = MAV_DISTANCE_SENSOR_LASER;
+				break;
+			}
+
+			uint8_t id = 0;
+			uint8_t orientation = 0;
+			uint8_t covariance = 20;
+
+			mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation,
+				range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance);
+		}
+	}
+};
+
+
+StreamListItem *streams_list[] = {
+	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
+	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
+	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
+	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
+	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
+	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
+	new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
+	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
+	new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
+	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
+	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
+	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
+	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
+	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
+	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
+	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
+	nullptr
+};
diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink/mavlink_messages.h
index c84b6fd26..ee64d0e42 100644
--- a/src/modules/mavlink_onboard/util.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.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
@@ -33,23 +32,30 @@
  ****************************************************************************/
 
 /**
- * @file util.h
- * Utility and helper functions and data.
+ * @file mavlink_messages.h
+ * MAVLink 1.0 message formatters definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#pragma once
+#ifndef MAVLINK_MESSAGES_H_
+#define MAVLINK_MESSAGES_H_
 
-#include "orb_topics.h"
+#include "mavlink_stream.h"
 
-/** MAVLink communications channel */
-extern uint8_t chan;
+class StreamListItem {
 
-/** Shutdown marker */
-extern volatile bool thread_should_exit;
+public:
+	MavlinkStream* (*new_instance)();
+	const char* (*get_name)();
 
-/**
- * Translate the custom state into standard mavlink modes and state.
- */
-extern void
-get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
-	uint8_t *mavlink_state, uint8_t *mavlink_mode);
+	StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+		new_instance(inst),
+		get_name(name) {};
+
+	~StreamListItem() {};
+};
+
+extern StreamListItem *streams_list[];
+
+#endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
new file mode 100644
index 000000000..068774c47
--- /dev/null
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -0,0 +1,828 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_mission.cpp
+ * MAVLink mission manager implementation.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_mission.h"
+#include "mavlink_main.h"
+
+#include <math.h>
+#include <lib/geo/geo.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include <dataman/dataman.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+
+MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
+	_mavlink(mavlink),
+	_state(MAVLINK_WPM_STATE_IDLE),
+	_time_last_recv(0),
+	_time_last_sent(0),
+	_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),
+	_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),
+	_max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX),
+	_dataman_id(0),
+	_count(0),
+	_current_seq(0),
+	_transfer_dataman_id(0),
+	_transfer_count(0),
+	_transfer_seq(0),
+	_transfer_current_seq(0),
+	_transfer_partner_sysid(0),
+	_transfer_partner_compid(0),
+	_offboard_mission_sub(-1),
+	_mission_result_sub(-1),
+	_offboard_mission_pub(-1),
+	_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
+	_verbose(false),
+	_channel(mavlink->get_channel()),
+	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
+{
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
+	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+
+	init_offboard_mission();
+}
+
+MavlinkMissionManager::~MavlinkMissionManager()
+{
+	close(_offboard_mission_pub);
+	close(_mission_result_sub);
+}
+
+void
+MavlinkMissionManager::init_offboard_mission()
+{
+	mission_s mission_state;
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
+		_dataman_id = mission_state.dataman_id;
+		_count = mission_state.count;
+		_current_seq = mission_state.current_seq;
+
+		warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq);
+
+	} else {
+		_dataman_id = 0;
+		_count = 0;
+		_current_seq = 0;
+		warnx("offboard mission init: ERROR, reading mission state failed");
+	}
+}
+
+/**
+ * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes.
+ */
+int
+MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq)
+{
+	struct mission_s mission;
+
+	mission.dataman_id = dataman_id;
+	mission.count = count;
+	mission.current_seq = seq;
+
+	/* update mission state in dataman */
+	int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
+
+	if (res == sizeof(mission_s)) {
+		/* update active mission state */
+		_dataman_id = dataman_id;
+		_count = count;
+		_current_seq = seq;
+
+		/* mission state saved successfully, publish offboard_mission topic */
+		if (_offboard_mission_pub < 0) {
+			_offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+
+		} else {
+			orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission);
+		}
+
+		return OK;
+
+	} else {
+		warnx("ERROR: can't save mission state");
+		_mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state");
+
+		return ERROR;
+	}
+}
+
+void
+MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
+{
+	mavlink_message_t msg;
+	mavlink_mission_ack_t wpa;
+
+	wpa.target_system = sysid;
+	wpa.target_component = compid;
+	wpa.type = type;
+
+	mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
+}
+
+
+void
+MavlinkMissionManager::send_mission_current(uint16_t seq)
+{
+	if (seq < _count) {
+		mavlink_message_t msg;
+		mavlink_mission_current_t wpc;
+
+		wpc.seq = seq;
+
+		mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
+		_mavlink->send_message(&msg);
+
+	} else if (seq == 0 && _count == 0) {
+		/* don't broadcast if no WPs */
+
+	} else {
+		if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); }
+
+		_mavlink->send_statustext_critical("ERROR: wp index out of bounds");
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count)
+{
+	_time_last_sent = hrt_absolute_time();
+
+	mavlink_message_t msg;
+	mavlink_mission_count_t wpc;
+
+	wpc.target_system = sysid;
+	wpc.target_component = compid;
+	wpc.count = _count;
+
+	mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
+}
+
+
+void
+MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
+{
+	dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id);
+	struct mission_item_s mission_item;
+
+	if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
+		_time_last_sent = hrt_absolute_time();
+
+		/* create mission_item_s from mavlink_mission_item_t */
+		mavlink_mission_item_t wp;
+		format_mavlink_mission_item(&mission_item, &wp);
+
+		mavlink_message_t msg;
+		wp.target_system = sysid;
+		wp.target_component = compid;
+		wp.seq = seq;
+		wp.current = (_current_seq == seq) ? 1 : 0;
+		mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
+		_mavlink->send_message(&msg);
+
+		if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
+
+	} else {
+		send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+		_mavlink->send_statustext_critical("Unable to read from micro SD");
+
+		if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); }
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq)
+{
+	if (seq < _max_count) {
+		_time_last_sent = hrt_absolute_time();
+
+		mavlink_message_t msg;
+		mavlink_mission_request_t wpr;
+		wpr.target_system = sysid;
+		wpr.target_component = compid;
+		wpr.seq = seq;
+		mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
+		_mavlink->send_message(&msg);
+
+		if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
+
+	} else {
+		_mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity");
+
+		if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); }
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
+{
+	mavlink_message_t msg;
+	mavlink_mission_item_reached_t wp_reached;
+
+	wp_reached.seq = seq;
+
+	mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
+}
+
+
+void
+MavlinkMissionManager::eventloop()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	bool updated = false;
+	orb_check(_mission_result_sub, &updated);
+
+	if (updated) {
+		mission_result_s mission_result;
+		orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result);
+
+		_current_seq = mission_result.seq_current;
+
+		if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); }
+
+		if (mission_result.reached) {
+			send_mission_item_reached((uint16_t)mission_result.seq_reached);
+		}
+
+		send_mission_current(_current_seq);
+
+	} else {
+		if (_slow_rate_limiter.check(now)) {
+			send_mission_current(_current_seq);
+		}
+	}
+
+	/* check for timed-out operations */
+	if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) {
+		_mavlink->send_statustext_critical("Operation timeout");
+
+		if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); }
+
+		_state = MAVLINK_WPM_STATE_IDLE;
+
+	} else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
+		/* try to request item again after timeout */
+		send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+
+	} else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
+		if (_transfer_seq == 0) {
+			/* try to send items count again after timeout */
+			send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count);
+
+		} else {
+			/* try to send item again after timeout */
+			send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1);
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_message(const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_MISSION_ACK:
+		handle_mission_ack(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
+		handle_mission_set_current(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+		handle_mission_request_list(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_REQUEST:
+		handle_mission_request(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_COUNT:
+		handle_mission_count(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_ITEM:
+		handle_mission_item(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+		handle_mission_clear_all(msg);
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
+{
+	mavlink_mission_ack_t wpa;
+	mavlink_msg_mission_ack_decode(msg, &wpa);
+
+	if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+		if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
+			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+				_time_last_recv = hrt_absolute_time();
+
+				if (_transfer_seq == _count) {
+					if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); }
+
+				} else {
+					_mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE");
+
+					if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); }
+				}
+
+				_state = MAVLINK_WPM_STATE_IDLE;
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
+
+			if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
+{
+	mavlink_mission_set_current_t wpc;
+	mavlink_msg_mission_set_current_decode(msg, &wpc);
+
+	if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wpc.seq < _count) {
+				if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) {
+					if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); }
+
+				} else {
+					if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); }
+
+					_mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID");
+				}
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); }
+
+				_mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list");
+			}
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); }
+
+			_mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy");
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
+{
+	mavlink_mission_request_list_t wprl;
+	mavlink_msg_mission_request_list_decode(msg, &wprl);
+
+	if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (_count > 0) {
+				_state = MAVLINK_WPM_STATE_SENDLIST;
+				_transfer_seq = 0;
+				_transfer_count = _count;
+				_transfer_partner_sysid = msg->sysid;
+				_transfer_partner_compid = msg->compid;
+
+				if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); }
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); }
+
+				_mavlink->send_statustext_info("WPM: mission is empty");
+			}
+
+			send_mission_count(msg->sysid, msg->compid, _count);
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); }
+
+			_mavlink->send_statustext_critical("IGN REQUEST LIST: Busy");
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
+{
+	mavlink_mission_request_t wpr;
+	mavlink_msg_mission_request_decode(msg, &wpr);
+
+	if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+		if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
+			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+				_time_last_recv = hrt_absolute_time();
+
+				/* _transfer_seq contains sequence of expected request */
+				if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); }
+
+					_transfer_seq++;
+
+				} else if (wpr.seq == _transfer_seq - 1) {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); }
+
+				} else {
+					if (_transfer_seq > 0 && _transfer_seq < _transfer_count) {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); }
+
+					} else if (_transfer_seq <= 0) {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); }
+
+					} else {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); }
+					}
+
+					_state = MAVLINK_WPM_STATE_IDLE;
+
+					send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+					_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
+					return;
+				}
+
+				/* double check bounds in case of items count changed */
+				if (wpr.seq < _count) {
+					send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq);
+
+				} else {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); }
+
+					_state = MAVLINK_WPM_STATE_IDLE;
+
+					send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+					_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
+				}
+
+			} else if (_state == MAVLINK_WPM_STATE_IDLE) {
+				if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); }
+
+				_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST: No active transfer");
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); }
+
+				_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch");
+
+			if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); }
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
+{
+	mavlink_mission_count_t wpc;
+	mavlink_msg_mission_count_decode(msg, &wpc);
+
+	if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wpc.count > _max_count) {
+				if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); }
+
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE);
+				return;
+			}
+
+			if (wpc.count == 0) {
+				if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+
+				/* alternate dataman ID anyway to let navigator know about changes */
+				update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
+				_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
+
+				// TODO send ACK?
+				return;
+			}
+
+			if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
+
+			_state = MAVLINK_WPM_STATE_GETLIST;
+			_transfer_seq = 0;
+			_transfer_partner_sysid = msg->sysid;
+			_transfer_partner_compid = msg->compid;
+			_transfer_count = wpc.count;
+			_transfer_dataman_id = _dataman_id == 0 ? 1 : 0;	// use inactive storage for transmission
+			_transfer_current_seq = -1;
+
+		} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (_transfer_seq == 0) {
+				/* looks like our MISSION_REQUEST was lost, try again */
+				if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); }
+
+				_mavlink->send_statustext_info("WP CMD OK TRY AGAIN");
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); }
+
+				_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
+				return;
+			}
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); }
+
+			_mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy");
+			return;
+		}
+
+		send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
+{
+	mavlink_mission_item_t wp;
+	mavlink_msg_mission_item_decode(msg, &wp);
+
+	if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
+		if (_state == MAVLINK_WPM_STATE_GETLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wp.seq != _transfer_seq) {
+				if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); }
+
+				/* don't send request here, it will be performed in eventloop after timeout */
+				return;
+			}
+
+		} else if (_state == MAVLINK_WPM_STATE_IDLE) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer");
+			return;
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
+			return;
+		}
+
+		struct mission_item_s mission_item;
+		int ret = parse_mavlink_mission_item(&wp, &mission_item);
+
+		if (ret != OK) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
+
+			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
+			_state = MAVLINK_WPM_STATE_IDLE;
+			return;
+		}
+
+		dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);
+
+		if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); }
+
+			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			_mavlink->send_statustext_critical("Unable to write on micro SD");
+			_state = MAVLINK_WPM_STATE_IDLE;
+			return;
+		}
+
+		/* waypoint marked as current */
+		if (wp.current) {
+			_transfer_current_seq = wp.seq;
+		}
+
+		if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }
+
+		_transfer_seq = wp.seq + 1;
+
+		if (_transfer_seq == _transfer_count) {
+			/* got all new mission items successfully */
+			if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }
+
+			_mavlink->send_statustext_info("WPM: Transfer complete.");
+
+			_state = MAVLINK_WPM_STATE_IDLE;
+
+			if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
+
+			} else {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			}
+
+		} else {
+			/* request next item */
+			send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
+{
+	mavlink_mission_clear_all_t wpca;
+	mavlink_msg_mission_clear_all_decode(msg, &wpca);
+
+	if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
+
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			/* don't touch mission items storage itself, but only items count in mission state */
+			_time_last_recv = hrt_absolute_time();
+
+			if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) {
+				if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
+
+			} else {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy");
+
+			if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); }
+		}
+	}
+}
+
+int
+MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
+{
+	/* only support global waypoints for now */
+	switch (mavlink_mission_item->frame) {
+	case MAV_FRAME_GLOBAL:
+		mission_item->lat = (double)mavlink_mission_item->x;
+		mission_item->lon = (double)mavlink_mission_item->y;
+		mission_item->altitude = mavlink_mission_item->z;
+		mission_item->altitude_is_relative = false;
+		break;
+
+	case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+		mission_item->lat = (double)mavlink_mission_item->x;
+		mission_item->lon = (double)mavlink_mission_item->y;
+		mission_item->altitude = mavlink_mission_item->z;
+		mission_item->altitude_is_relative = true;
+		break;
+
+	case MAV_FRAME_LOCAL_NED:
+	case MAV_FRAME_LOCAL_ENU:
+		return MAV_MISSION_UNSUPPORTED_FRAME;
+
+	case MAV_FRAME_MISSION:
+	default:
+		return MAV_MISSION_ERROR;
+	}
+
+	switch (mavlink_mission_item->command) {
+	case MAV_CMD_NAV_TAKEOFF:
+		mission_item->pitch_min = mavlink_mission_item->param1;
+		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:
+		mission_item->acceptance_radius = mavlink_mission_item->param2;
+		mission_item->time_inside = mavlink_mission_item->param1;
+		break;
+	}
+
+	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+	mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+
+	mission_item->autocontinue = mavlink_mission_item->autocontinue;
+	// mission_item->index = mavlink_mission_item->seq;
+	mission_item->origin = ORIGIN_MAVLINK;
+
+	/* reset DO_JUMP count */
+	mission_item->do_jump_current_count = 0;
+
+	return MAV_MISSION_ACCEPTED;
+}
+
+
+int
+MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
+{
+	if (mission_item->altitude_is_relative) {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+
+	} else {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	}
+
+	switch (mission_item->nav_cmd) {
+	case NAV_CMD_TAKEOFF:
+		mavlink_mission_item->param1 = mission_item->pitch_min;
+		break;
+
+	case NAV_CMD_DO_JUMP:
+		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		break;
+
+	default:
+		mavlink_mission_item->param2 = mission_item->acceptance_radius;
+		mavlink_mission_item->param1 = mission_item->time_inside;
+		break;
+	}
+
+	mavlink_mission_item->x = (float)mission_item->lat;
+	mavlink_mission_item->y = (float)mission_item->lon;
+	mavlink_mission_item->z = mission_item->altitude;
+
+	mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+	mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+	mavlink_mission_item->command = mission_item->nav_cmd;
+	mavlink_mission_item->autocontinue = mission_item->autocontinue;
+	// mavlink_mission_item->seq = mission_item->index;
+
+	return OK;
+}
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
new file mode 100644
index 000000000..f63c32f24
--- /dev/null
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_mission.h
+ * MAVLink mission manager interface definition.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include "mavlink_bridge_header.h"
+#include "mavlink_rate_limiter.h"
+#include <uORB/uORB.h>
+
+// FIXME XXX - TO BE MOVED TO XML
+enum MAVLINK_WPM_STATES {
+	MAVLINK_WPM_STATE_IDLE = 0,
+	MAVLINK_WPM_STATE_SENDLIST,
+	MAVLINK_WPM_STATE_GETLIST,
+	MAVLINK_WPM_STATE_ENUM_END
+};
+
+enum MAVLINK_WPM_CODES {
+	MAVLINK_WPM_CODE_OK = 0,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
+	MAVLINK_WPM_CODE_ENUM_END
+};
+
+#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000    ///< Protocol communication action timeout in useconds
+#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000        ///< Protocol communication retry timeout in useconds
+
+class Mavlink;
+
+class MavlinkMissionManager {
+public:
+	MavlinkMissionManager(Mavlink *parent);
+
+	~MavlinkMissionManager();
+
+	void init_offboard_mission();
+
+	int update_active_mission(int dataman_id, unsigned count, int seq);
+
+	void send_message(mavlink_message_t *msg);
+
+	/**
+	 *  @brief Sends an waypoint ack message
+	 */
+	void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type);
+
+	/**
+	 *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
+	 *
+	 *  This function broadcasts its new active waypoint sequence number and
+	 *  sends a message to the controller, advising it to fly to the coordinates
+	 *  of the waypoint with a given orientation
+	 *
+	 *  @param seq The waypoint sequence number the MAV should fly to.
+	 */
+	void send_mission_current(uint16_t seq);
+
+	void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count);
+
+	void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq);
+
+	void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq);
+
+	/**
+	 *  @brief emits a message that a waypoint reached
+	 *
+	 *  This function broadcasts a message that a waypoint is reached.
+	 *
+	 *  @param seq The waypoint sequence number the MAV has reached.
+	 */
+	void send_mission_item_reached(uint16_t seq);
+
+	void eventloop();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	void handle_mission_ack(const mavlink_message_t *msg);
+
+	void handle_mission_set_current(const mavlink_message_t *msg);
+
+	void handle_mission_request_list(const mavlink_message_t *msg);
+
+	void handle_mission_request(const mavlink_message_t *msg);
+
+	void handle_mission_count(const mavlink_message_t *msg);
+
+	void handle_mission_item(const mavlink_message_t *msg);
+
+	void handle_mission_clear_all(const mavlink_message_t *msg);
+
+	/**
+	 * Parse mavlink MISSION_ITEM message to get mission_item_s.
+	 */
+	int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
+
+	/**
+	 * Format mission_item_s as mavlink MISSION_ITEM message.
+	 */
+	int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
+
+	void set_verbose(bool v) { _verbose = v; }
+
+private:
+	Mavlink*		_mavlink;
+
+	enum MAVLINK_WPM_STATES _state;					///< Current state
+
+	uint64_t		_time_last_recv;
+	uint64_t		_time_last_sent;
+
+	uint32_t		_action_timeout;
+	uint32_t		_retry_timeout;
+	unsigned		_max_count;				///< Maximum number of mission items
+
+	int			_dataman_id;				///< Dataman storage ID for active mission
+	unsigned		_count;					///< Count of items in active mission
+	int			_current_seq;				///< Current item sequence in active mission
+
+	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
+	unsigned		_transfer_count;			///< Items count in current transmission
+	unsigned		_transfer_seq;				///< Item sequence in current transmission
+	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
+	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
+	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
+
+	int			_offboard_mission_sub;
+	int			_mission_result_sub;
+	orb_advert_t		_offboard_mission_pub;
+
+	MavlinkRateLimiter	_slow_rate_limiter;
+
+	bool _verbose;
+
+	mavlink_channel_t	_channel;
+	uint8_t			_comp_id;
+};
diff --git a/src/modules/sdlog/sdlog_ringbuffer.c b/src/modules/mavlink/mavlink_orb_subscription.cpp
index d7c8a4759..734f0903a 100644
--- a/src/modules/sdlog/sdlog_ringbuffer.c
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -33,59 +32,88 @@
  ****************************************************************************/
 
 /**
- * @file sdlog_log.c
- * MAVLink text logging.
+ * @file mavlink_orb_subscription.cpp
+ * uORB subscription implementation.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <string.h>
+#include <unistd.h>
 #include <stdlib.h>
+#include <string.h>
+#include <uORB/uORB.h>
+#include <stdio.h>
 
-#include "sdlog_ringbuffer.h"
+#include "mavlink_orb_subscription.h"
 
-void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+	next(nullptr),
+	_topic(topic),
+	_fd(orb_subscribe(_topic)),
+	_published(false)
 {
-	lb->size  = size;
-	lb->start = 0;
-	lb->count = 0;
-	lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector));
 }
 
-int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb)
+MavlinkOrbSubscription::~MavlinkOrbSubscription()
 {
-	return lb->count == (int)lb->size;
+	close(_fd);
 }
 
-int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb)
+orb_id_t
+MavlinkOrbSubscription::get_topic() const
 {
-	return lb->count == 0;
+	return _topic;
 }
 
-
-// XXX make these functions thread-safe
-void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem)
+bool
+MavlinkOrbSubscription::update(uint64_t *time, void* data)
 {
-	int end = (lb->start + lb->count) % lb->size;
-	memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector));
+	// TODO this is NOT atomic operation, we can get data newer than time
+	// if topic was published between orb_stat and orb_copy calls.
 
-	if (sdlog_logbuffer_is_full(lb)) {
-		lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
+	uint64_t time_topic;
+	if (orb_stat(_fd, &time_topic)) {
+		/* error getting last topic publication time */
+		time_topic = 0;
+	}
+
+	if (orb_copy(_topic, _fd, data)) {
+		/* error copying topic data */
+		memset(data, 0, _topic->o_size);
+		return false;
 
 	} else {
-		++lb->count;
+		/* data copied successfully */
+		_published = true;
+		if (time_topic != *time) {
+			*time = time_topic;
+			return true;
+
+		} else {
+			return false;
+		}
 	}
 }
 
-int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem)
+bool
+MavlinkOrbSubscription::update(void* data)
 {
-	if (!sdlog_logbuffer_is_empty(lb)) {
-		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector));
-		lb->start = (lb->start + 1) % lb->size;
-		--lb->count;
-		return 0;
+	return !orb_copy(_topic, _fd, data);
+}
 
-	} else {
-		return 1;
+bool
+MavlinkOrbSubscription::is_published()
+{
+	if (_published) {
+		return true;
+	}
+
+	bool updated;
+	orb_check(_fd, &updated);
+
+	if (updated) {
+		_published = true;
 	}
+
+	return _published;
 }
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
new file mode 100644
index 000000000..71efb43af
--- /dev/null
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   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_orb_subscription.h
+ * uORB subscription definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
+#define MAVLINK_ORB_SUBSCRIPTION_H_
+
+#include <systemlib/uthash/utlist.h>
+#include <drivers/drv_hrt.h>
+
+
+class MavlinkOrbSubscription
+{
+public:
+	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
+
+	MavlinkOrbSubscription(const orb_id_t topic);
+	~MavlinkOrbSubscription();
+
+	/**
+	 * Check if subscription updated and get data.
+	 *
+	 * @return true only if topic was updated and data copied to buffer successfully.
+	 * If topic was not updated since last check it will return false but still copy the data.
+	 * If no data available data buffer will be filled with zeroes.
+	 */
+	bool update(uint64_t *time, void* data);
+
+	/**
+	 * Copy topic data to given buffer.
+	 *
+	 * @return true only if topic data copied successfully.
+	 */
+	bool update(void* data);
+
+	/**
+	 * Check if the topic has been published.
+	 *
+	 * This call will return true if the topic was ever published.
+	 * @return true if the topic has been published at least once.
+	 */
+	bool is_published();
+	orb_id_t get_topic() const;
+
+private:
+	const orb_id_t _topic;		///< topic metadata
+	int _fd;			///< subscription handle
+	bool _published;		///< topic was ever published
+};
+
+
+#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */
diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c
deleted file mode 100644
index 18ca7a854..000000000
--- a/src/modules/mavlink/mavlink_parameters.c
+++ /dev/null
@@ -1,230 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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
- * 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_parameters.c
- * MAVLink parameter protocol implementation (BSD-relicensed).
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include "mavlink_bridge_header.h"
-#include "mavlink_parameters.h"
-#include <uORB/uORB.h>
-#include "math.h" /* isinf / isnan checks */
-#include <assert.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <stdbool.h>
-#include <string.h>
-#include <errno.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <sys/stat.h>
-
-extern mavlink_system_t mavlink_system;
-
-extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern int mavlink_missionlib_send_gcs_string(const char *string);
-
-/**
- * If the queue index is not at 0, the queue sending
- * logic will send parameters from the current index
- * to len - 1, the end of the param list.
- */
-static unsigned int mavlink_param_queue_index = 0;
-
-/**
- * Callback for param interface.
- */
-void mavlink_pm_callback(void *arg, param_t param);
-
-void mavlink_pm_callback(void *arg, param_t param)
-{
-	mavlink_pm_send_param(param);
-	usleep(*(unsigned int *)arg);
-}
-
-void mavlink_pm_send_all_params(unsigned int delay)
-{
-	unsigned int dbuf = delay;
-	param_foreach(&mavlink_pm_callback, &dbuf, false);
-}
-
-int mavlink_pm_queued_send()
-{
-	if (mavlink_param_queue_index < param_count()) {
-		mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
-		mavlink_param_queue_index++;
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-void mavlink_pm_start_queued_send()
-{
-	mavlink_param_queue_index = 0;
-}
-
-int mavlink_pm_send_param_for_index(uint16_t index)
-{
-	return mavlink_pm_send_param(param_for_index(index));
-}
-
-int mavlink_pm_send_param_for_name(const char *name)
-{
-	return mavlink_pm_send_param(param_find(name));
-}
-
-int mavlink_pm_send_param(param_t param)
-{
-	if (param == PARAM_INVALID) return 1;
-
-	/* buffers for param transmission */
-	static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
-	float val_buf;
-	static mavlink_message_t tx_msg;
-
-	/* query parameter type */
-	param_type_t type = param_type(param);
-	/* copy parameter name */
-	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-
-	/*
-	 * Map onboard parameter type to MAVLink type,
-	 * endianess matches (both little endian)
-	 */
-	uint8_t mavlink_type;
-
-	if (type == PARAM_TYPE_INT32) {
-		mavlink_type = MAVLINK_TYPE_INT32_T;
-
-	} else if (type == PARAM_TYPE_FLOAT) {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-
-	} else {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-	}
-
-	/*
-	 * get param value, since MAVLink encodes float and int params in the same
-	 * space during transmission, copy param onto float val_buf
-	 */
-
-	int ret;
-
-	if ((ret = param_get(param, &val_buf)) != OK) {
-		return ret;
-	}
-
-	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
-					  mavlink_system.compid,
-					  MAVLINK_COMM_0,
-					  &tx_msg,
-					  name_buf,
-					  val_buf,
-					  mavlink_type,
-					  param_count(),
-					  param_get_index(param));
-	ret = mavlink_missionlib_send_message(&tx_msg);
-	return ret;
-}
-
-void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
-{
-	switch (msg->msgid) {
-	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
-			/* Start sending parameters */
-			mavlink_pm_start_queued_send();
-			mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_SET: {
-
-			/* Handle parameter setting */
-
-			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
-				mavlink_param_set_t mavlink_param_set;
-				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
-
-				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter, set and send it */
-					param_t param = param_find(name);
-
-					if (param == PARAM_INVALID) {
-						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[mavlink pm] unknown: %s", name);
-						mavlink_missionlib_send_gcs_string(buf);
-
-					} else {
-						/* set and send parameter */
-						param_set(param, &(mavlink_param_set.param_value));
-						mavlink_pm_send_param(param);
-					}
-				}
-			}
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
-			mavlink_param_request_read_t mavlink_param_request_read;
-			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
-
-			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
-				/* when no index is given, loop through string ids and compare them */
-				if (mavlink_param_request_read.param_index == -1) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter and send it */
-					mavlink_pm_send_param_for_name(name);
-
-				} else {
-					/* when index is >= 0, send this parameter again */
-					mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
-				}
-			}
-
-		} break;
-	}
-}
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
deleted file mode 100644
index b1e38bcc8..000000000
--- a/src/modules/mavlink/mavlink_parameters.h
+++ /dev/null
@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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
- * 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_parameters.h
- * MAVLink parameter protocol definitions (BSD-relicensed).
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* This assumes you have the mavlink headers on your include path
- or in the same folder as this source file */
-
-
-#include <v1.0/mavlink_types.h>
-#include <stdbool.h>
-#include <systemlib/param/param.h>
-
-/**
- * Handle parameter related messages.
- */
-void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
-
-/**
- * Send all parameters at once.
- *
- * This function blocks until all parameters have been sent.
- * it delays each parameter by the passed amount of microseconds.
- *
- * @param delay		The delay in us between sending all parameters.
- */
-void mavlink_pm_send_all_params(unsigned int delay);
-
-/**
- * Send one parameter.
- *
- * @param param		The parameter id to send.
- * @return		zero on success, nonzero on failure.
- */
-int mavlink_pm_send_param(param_t param);
-
-/**
- * Send one parameter identified by index.
- *
- * @param index		The index of the parameter to send.
- * @return		zero on success, nonzero else.
- */
-int mavlink_pm_send_param_for_index(uint16_t index);
-
-/**
- * Send one parameter identified by name.
- *
- * @param name		The index of the parameter to send.
- * @return		zero on success, nonzero else.
- */
-int mavlink_pm_send_param_for_name(const char *name);
-
-/**
- * Send a queue of parameters, one parameter per function call.
- *
- * @return		zero on success, nonzero on failure
- */
-int mavlink_pm_queued_send(void);
-
-/**
- * Start sending the parameter queue.
- *
- * This function will not directly send parameters, but instead
- * activate the sending of one parameter on each call of
- * mavlink_pm_queued_send().
- * @see 		mavlink_pm_queued_send()
- */
-void mavlink_pm_start_queued_send(void);
diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/modules/mavlink/mavlink_rate_limiter.cpp
index 4b84523cc..9cdda8b17 100644
--- a/src/systemcmds/hw_ver/hw_ver.c
+++ b/src/modules/mavlink/mavlink_rate_limiter.cpp
@@ -32,42 +32,41 @@
  ****************************************************************************/
 
 /**
- * @file hw_ver.c
+ * @file mavlink_rate_limiter.cpp
+ * Message rate limiter implementation.
  *
- * Show and test hardware version.
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <nuttx/config.h>
+#include "mavlink_rate_limiter.h"
 
-#include <stdio.h>
-#include <string.h>
-#include <errno.h>
-#include <version/version.h>
+MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
+{
+}
 
-__EXPORT int hw_ver_main(int argc, char *argv[]);
+MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval)
+{
+}
 
-int
-hw_ver_main(int argc, char *argv[])
+MavlinkRateLimiter::~MavlinkRateLimiter()
 {
-	if (argc >= 2) {
-		if (!strcmp(argv[1], "show")) {
-			printf(HW_ARCH "\n");
-			exit(0);
-		}
+}
 
-		if (!strcmp(argv[1], "compare")) {
-			if (argc >= 3) {
-				int ret = strcmp(HW_ARCH, argv[2]) != 0;
-				if (ret == 0) {
-					printf("hw_ver match: %s\n", HW_ARCH);
-				}
-				exit(ret);
+void
+MavlinkRateLimiter::set_interval(unsigned int interval)
+{
+	_interval = interval;
+}
+
+bool
+MavlinkRateLimiter::check(hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
 
-			} else {
-				errx(1, "not enough arguments, try 'compare PX4FMU_1'");
-			}
-		}
+	if (dt > 0 && dt >= _interval) {
+		_last_sent = (t / _interval) * _interval;
+		return true;
 	}
 
-	errx(1, "expected a command, try 'show' or 'compare'");
+	return false;
 }
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/mavlink_rate_limiter.h
index 5e5ee8261..0b37538e6 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/mavlink_rate_limiter.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -33,22 +32,31 @@
  ****************************************************************************/
 
 /**
- * @file util.h
- * Utility and helper functions and data.
+ * @file mavlink_rate_limiter.h
+ * Message rate limiter definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#pragma once
+#ifndef MAVLINK_RATE_LIMITER_H_
+#define MAVLINK_RATE_LIMITER_H_
 
-/** MAVLink communications channel */
-extern uint8_t chan;
+#include <drivers/drv_hrt.h>
 
-/** Shutdown marker */
-extern volatile bool thread_should_exit;
 
-/** Waypoint storage */
-extern mavlink_wpm_storage *wpm;
+class MavlinkRateLimiter
+{
+private:
+	hrt_abstime _last_sent;
+	hrt_abstime _interval;
 
-/**
- * Translate the custom state into standard mavlink modes and state.
- */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+public:
+	MavlinkRateLimiter();
+	MavlinkRateLimiter(unsigned int interval);
+	~MavlinkRateLimiter();
+	void set_interval(unsigned int interval);
+	bool check(hrt_abstime t);
+};
+
+
+#endif /* MAVLINK_RATE_LIMITER_H_ */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7b6fad658..99ec98ee9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.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
@@ -33,10 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_receiver.c
+ * @file mavlink_receiver.cpp
  * MAVLink protocol message receive and dispatch
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 /* XXX trim includes */
@@ -77,272 +77,334 @@
 __BEGIN_DECLS
 
 #include "mavlink_bridge_header.h"
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "mavlink_parameters.h"
-#include "util.h"
-
-extern bool gcs_link;
+#include "mavlink_receiver.h"
+#include "mavlink_main.h"
 
 __END_DECLS
 
-/* XXX should be in a header somewhere */
-extern "C" pthread_t receive_start(int uart);
-
-static void handle_message(mavlink_message_t *msg);
-static void *receive_thread(void *arg);
-
-static mavlink_status_t status;
-static struct vehicle_vicon_position_s vicon_position;
-static struct vehicle_command_s vcmd;
-static struct offboard_control_setpoint_s offboard_control_sp;
-
-struct vehicle_global_position_s hil_global_pos;
-struct vehicle_local_position_s hil_local_pos;
-struct vehicle_attitude_s hil_attitude;
-struct vehicle_gps_position_s hil_gps;
-struct sensor_combined_s hil_sensors;
-struct battery_status_s	hil_battery_status;
-static orb_advert_t pub_hil_global_pos = -1;
-static orb_advert_t pub_hil_local_pos = -1;
-static orb_advert_t pub_hil_attitude = -1;
-static orb_advert_t pub_hil_gps = -1;
-static orb_advert_t pub_hil_sensors = -1;
-static orb_advert_t pub_hil_gyro = -1;
-static orb_advert_t pub_hil_accel = -1;
-static orb_advert_t pub_hil_mag = -1;
-static orb_advert_t pub_hil_baro = -1;
-static orb_advert_t pub_hil_airspeed = -1;
-static orb_advert_t pub_hil_battery = -1;
-
-/* packet counter */
-static int hil_counter = 0;
-static int hil_frames = 0;
-static uint64_t old_timestamp = 0;
-
-static orb_advert_t cmd_pub = -1;
-static orb_advert_t flow_pub = -1;
-
-static orb_advert_t offboard_control_sp_pub = -1;
-static orb_advert_t vicon_position_pub = -1;
-static orb_advert_t telemetry_status_pub = -1;
-
-// variables for HIL reference position
-static int32_t lat0 = 0;
-static int32_t lon0 = 0;
-static double alt0 = 0;
-
-static void
-handle_message(mavlink_message_t *msg)
+static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
+
+MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
+	_mavlink(parent),
+
+	_global_pos_pub(-1),
+	_local_pos_pub(-1),
+	_attitude_pub(-1),
+	_gps_pub(-1),
+	_sensors_pub(-1),
+	_gyro_pub(-1),
+	_accel_pub(-1),
+	_mag_pub(-1),
+	_baro_pub(-1),
+	_airspeed_pub(-1),
+	_battery_pub(-1),
+	_cmd_pub(-1),
+	_flow_pub(-1),
+	_offboard_control_sp_pub(-1),
+	_local_pos_sp_pub(-1),
+	_global_vel_sp_pub(-1),
+	_att_sp_pub(-1),
+	_rates_sp_pub(-1),
+	_vicon_position_pub(-1),
+	_telemetry_status_pub(-1),
+	_rc_pub(-1),
+	_manual_pub(-1),
+	_telemetry_heartbeat_time(0),
+	_radio_status_available(false),
+	_control_mode_sub(-1),
+	_hil_frames(0),
+	_old_timestamp(0),
+	_hil_local_proj_inited(0),
+	_hil_local_alt0(0.0)
 {
-	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
+	memset(&_control_mode, 0, sizeof(_control_mode));
 
-		mavlink_command_long_t cmd_mavlink;
-		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
+	// make sure the FTP server is started
+	(void)MavlinkFTP::getServer();
+}
 
-		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
-				|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
-			//check for MAVLINK terminate command
-			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
-				/* This is the link shutdown command, terminate mavlink */
-				printf("[mavlink] Terminating .. \n");
-				fflush(stdout);
-				usleep(50000);
+MavlinkReceiver::~MavlinkReceiver()
+{
+}
 
-				/* terminate other threads and this thread */
-				thread_should_exit = true;
+void
+MavlinkReceiver::handle_message(mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_COMMAND_LONG:
+		handle_message_command_long(msg);
+		break;
 
-			} else {
+	case MAVLINK_MSG_ID_OPTICAL_FLOW:
+		handle_message_optical_flow(msg);
+		break;
 
-				/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-				vcmd.param1 = cmd_mavlink.param1;
-				vcmd.param2 = cmd_mavlink.param2;
-				vcmd.param3 = cmd_mavlink.param3;
-				vcmd.param4 = cmd_mavlink.param4;
-				vcmd.param5 = cmd_mavlink.param5;
-				vcmd.param6 = cmd_mavlink.param6;
-				vcmd.param7 = cmd_mavlink.param7;
-				// XXX do proper translation
-				vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
-				vcmd.target_system = cmd_mavlink.target_system;
-				vcmd.target_component = cmd_mavlink.target_component;
-				vcmd.source_system = msg->sysid;
-				vcmd.source_component = msg->compid;
-				vcmd.confirmation =  cmd_mavlink.confirmation;
-
-				/* check if topic is advertised */
-				if (cmd_pub <= 0) {
-					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+	case MAVLINK_MSG_ID_SET_MODE:
+		handle_message_set_mode(msg);
+		break;
 
-				} else {
-					/* publish */
-					orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-				}
-			}
-		}
-	}
+	case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
+		handle_message_vicon_position_estimate(msg);
+		break;
 
-	if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
-		mavlink_optical_flow_t flow;
-		mavlink_msg_optical_flow_decode(msg, &flow);
+	case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
+		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
+		break;
 
-		struct optical_flow_s f;
+	case MAVLINK_MSG_ID_RADIO_STATUS:
+		handle_message_radio_status(msg);
+		break;
 
-		f.timestamp = flow.time_usec;
-		f.flow_raw_x = flow.flow_x;
-		f.flow_raw_y = flow.flow_y;
-		f.flow_comp_x_m = flow.flow_comp_m_x;
-		f.flow_comp_y_m = flow.flow_comp_m_y;
-		f.ground_distance_m = flow.ground_distance;
-		f.quality = flow.quality;
-		f.sensor_id = flow.sensor_id;
+	case MAVLINK_MSG_ID_MANUAL_CONTROL:
+		handle_message_manual_control(msg);
+		break;
 
-		/* check if topic is advertised */
-		if (flow_pub <= 0) {
-			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+	case MAVLINK_MSG_ID_HEARTBEAT:
+		handle_message_heartbeat(msg);
+		break;
 
-		} else {
-			/* publish */
-			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
-		}
-	}
+	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+		handle_message_request_data_stream(msg);
+		break;
 
-	if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
-		/* Set mode on request */
-		mavlink_set_mode_t new_mode;
-		mavlink_msg_set_mode_decode(msg, &new_mode);
-
-		union px4_custom_mode custom_mode;
-		custom_mode.data = new_mode.custom_mode;
-		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = custom_mode.main_mode;
-		vcmd.param3 = 0;
-		vcmd.param4 = 0;
-		vcmd.param5 = 0;
-		vcmd.param6 = 0;
-		vcmd.param7 = 0;
-		vcmd.command = VEHICLE_CMD_DO_SET_MODE;
-		vcmd.target_system = new_mode.target_system;
-		vcmd.target_component = MAV_COMP_ID_ALL;
-		vcmd.source_system = msg->sysid;
-		vcmd.source_component = msg->compid;
-		vcmd.confirmation = 1;
-
-		/* check if topic is advertised */
-		if (cmd_pub <= 0) {
-			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
+		break;
 
-		} else {
-			/* create command */
-			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-		}
+	default:
+		break;
 	}
 
-	/* Handle Vicon position estimates */
-	if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
-		mavlink_vicon_position_estimate_t pos;
-		mavlink_msg_vicon_position_estimate_decode(msg, &pos);
-
-		vicon_position.timestamp = hrt_absolute_time();
-
-		vicon_position.x = pos.x;
-		vicon_position.y = pos.y;
-		vicon_position.z = pos.z;
+	/*
+	 * Only decode hil messages in HIL mode.
+	 *
+	 * The HIL mode is enabled by the HIL bit flag
+	 * in the system mode. Either send a set mode
+	 * COMMAND_LONG message or a SET_MODE message
+	 *
+	 * Accept HIL GPS messages if use_hil_gps flag is true.
+	 * This allows to provide fake gps measurements to the system.
+	 */
+	if (_mavlink->get_hil_enabled()) {
+		switch (msg->msgid) {
+		case MAVLINK_MSG_ID_HIL_SENSOR:
+			handle_message_hil_sensor(msg);
+			break;
+
+		case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
+			handle_message_hil_state_quaternion(msg);
+			break;
+
+		default:
+			break;
+		}
+	}
 
-		vicon_position.roll = pos.roll;
-		vicon_position.pitch = pos.pitch;
-		vicon_position.yaw = pos.yaw;
 
-		if (vicon_position_pub <= 0) {
-			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+   	if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
+		switch (msg->msgid) {
+		case MAVLINK_MSG_ID_HIL_GPS:
+			handle_message_hil_gps(msg);
+			break;
 
-		} else {
-			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
+		default:
+			break;
 		}
+
 	}
 
-	/* Handle quadrotor motor setpoints */
+    /* 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 (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
-		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
-		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
+void
+MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
+{
+	/* command */
+	mavlink_command_long_t cmd_mavlink;
+	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
+
+	if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+		//check for MAVLINK terminate command
+		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
+			/* This is the link shutdown command, terminate mavlink */
+			warnx("terminated by remote command");
+			fflush(stdout);
+			usleep(50000);
+
+			/* terminate other threads and this thread */
+			_mavlink->_task_should_exit = true;
 
-		if (mavlink_system.sysid < 4) {
+		} else {
 
-			/* switch to a receiving link mode */
-			gcs_link = false;
+			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+				warnx("ignoring CMD spoofed with same SYS/COMP ID");
+				return;
+			}
 
-			/*
-			 * rate control mode - defined by MAVLink
-			 */
+			struct vehicle_command_s vcmd;
+			memset(&vcmd, 0, sizeof(vcmd));
+
+			/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
+			vcmd.param1 = cmd_mavlink.param1;
+			vcmd.param2 = cmd_mavlink.param2;
+			vcmd.param3 = cmd_mavlink.param3;
+			vcmd.param4 = cmd_mavlink.param4;
+			vcmd.param5 = cmd_mavlink.param5;
+			vcmd.param6 = cmd_mavlink.param6;
+			vcmd.param7 = cmd_mavlink.param7;
+			// XXX do proper translation
+			vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
+			vcmd.target_system = cmd_mavlink.target_system;
+			vcmd.target_component = cmd_mavlink.target_component;
+			vcmd.source_system = msg->sysid;
+			vcmd.source_component = msg->compid;
+			vcmd.confirmation =  cmd_mavlink.confirmation;
+
+			if (_cmd_pub < 0) {
+				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
-			uint8_t ml_mode = 0;
-			bool ml_armed = false;
+			} else {
+				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+			}
+		}
+	}
+}
 
-			switch (quad_motors_setpoint.mode) {
-			case 0:
-				ml_armed = false;
-				break;
+void
+MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+{
+	/* optical flow */
+	mavlink_optical_flow_t flow;
+	mavlink_msg_optical_flow_decode(msg, &flow);
+
+	struct optical_flow_s f;
+	memset(&f, 0, sizeof(f));
+
+	f.timestamp = hrt_absolute_time();
+	f.flow_timestamp = flow.time_usec;
+	f.flow_raw_x = flow.flow_x;
+	f.flow_raw_y = flow.flow_y;
+	f.flow_comp_x_m = flow.flow_comp_m_x;
+	f.flow_comp_y_m = flow.flow_comp_m_y;
+	f.ground_distance_m = flow.ground_distance;
+	f.quality = flow.quality;
+	f.sensor_id = flow.sensor_id;
+
+	if (_flow_pub < 0) {
+		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+
+	} else {
+		orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
+	}
+}
 
-			case 1:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-				ml_armed = true;
+void
+MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
+{
+	mavlink_set_mode_t new_mode;
+	mavlink_msg_set_mode_decode(msg, &new_mode);
+
+	struct vehicle_command_s vcmd;
+	memset(&vcmd, 0, sizeof(vcmd));
+
+	union px4_custom_mode custom_mode;
+	custom_mode.data = new_mode.custom_mode;
+	/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
+	vcmd.param1 = new_mode.base_mode;
+	vcmd.param2 = custom_mode.main_mode;
+	vcmd.param3 = 0;
+	vcmd.param4 = 0;
+	vcmd.param5 = 0;
+	vcmd.param6 = 0;
+	vcmd.param7 = 0;
+	vcmd.command = VEHICLE_CMD_DO_SET_MODE;
+	vcmd.target_system = new_mode.target_system;
+	vcmd.target_component = MAV_COMP_ID_ALL;
+	vcmd.source_system = msg->sysid;
+	vcmd.source_component = msg->compid;
+	vcmd.confirmation = 1;
+
+	if (_cmd_pub < 0) {
+		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
+	} else {
+		orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+	}
+}
 
-				break;
+void
+MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
+{
+	mavlink_vicon_position_estimate_t pos;
+	mavlink_msg_vicon_position_estimate_decode(msg, &pos);
 
-			case 2:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-				ml_armed = true;
+	struct vehicle_vicon_position_s vicon_position;
+	memset(&vicon_position, 0, sizeof(vicon_position));
 
-				break;
+	vicon_position.timestamp = hrt_absolute_time();
+	vicon_position.x = pos.x;
+	vicon_position.y = pos.y;
+	vicon_position.z = pos.z;
+	vicon_position.roll = pos.roll;
+	vicon_position.pitch = pos.pitch;
+	vicon_position.yaw = pos.yaw;
 
-			case 3:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-				break;
+	if (_vicon_position_pub < 0) {
+		_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
 
-			case 4:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-				break;
-			}
+	} else {
+		orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position);
+	}
+}
 
-			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
+void
+MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
+{
+	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
+	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
 
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
-				ml_armed = false;
-			}
+	/* Only accept system IDs from 1 to 4 */
+	if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
+		struct offboard_control_setpoint_s offboard_control_sp;
+		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
 
-			offboard_control_sp.armed = ml_armed;
-			offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
+		/* Convert values * 1000 back */
+		offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
 
-			offboard_control_sp.timestamp = hrt_absolute_time();
+		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
 
-			/* check if topic has to be advertised */
-			if (offboard_control_sp_pub <= 0) {
-				offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+		offboard_control_sp.timestamp = hrt_absolute_time();
 
-			} else {
-				/* Publish */
-				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
-			}
+		if (_offboard_control_sp_pub < 0) {
+			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+		} else {
+			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
 		}
 	}
+}
 
-	/* handle status updates of the radio */
-	if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
-
-		struct telemetry_status_s tstatus;
-
+void
+MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
+{
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
+	if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
 		mavlink_radio_status_t rstatus;
 		mavlink_msg_radio_status_decode(msg, &rstatus);
 
-		/* publish telemetry status topic */
+		struct telemetry_status_s tstatus;
+		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;
@@ -352,455 +414,499 @@ handle_message(mavlink_message_t *msg)
 		tstatus.rxerrors = rstatus.rxerrors;
 		tstatus.fixed = rstatus.fixed;
 
-		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(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
 		} else {
-			orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus);
+			orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 		}
-	}
-
-	/*
-	 * Only decode hil messages in HIL mode.
-	 *
-	 * The HIL mode is enabled by the HIL bit flag
-	 * in the system mode. Either send a set mode
-	 * COMMAND_LONG message or a SET_MODE message
-	 */
 
-	if (mavlink_hil_enabled) {
-
-		uint64_t timestamp = hrt_absolute_time();
-
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) {
-
-			mavlink_hil_sensor_t imu;
-			mavlink_msg_hil_sensor_decode(msg, &imu);
-
-			/* sensors general */
-			hil_sensors.timestamp = hrt_absolute_time();
-
-			/* hil gyro */
-			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
-			hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
-			hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
-			hil_sensors.gyro_rad_s[0] = imu.xgyro;
-			hil_sensors.gyro_rad_s[1] = imu.ygyro;
-			hil_sensors.gyro_rad_s[2] = imu.zgyro;
-			hil_sensors.gyro_counter = hil_counter;
-
-			/* accelerometer */
-			static const float mg2ms2 = 9.8f / 1000.0f;
-			hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
-			hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
-			hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
-			hil_sensors.accelerometer_m_s2[0] = imu.xacc;
-			hil_sensors.accelerometer_m_s2[1] = imu.yacc;
-			hil_sensors.accelerometer_m_s2[2] = imu.zacc;
-			hil_sensors.accelerometer_mode = 0; // TODO what is this?
-			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
-			hil_sensors.accelerometer_counter = hil_counter;
-
-			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0.0f;
-			hil_sensors.adc_voltage_v[1] = 0.0f;
-			hil_sensors.adc_voltage_v[2] = 0.0f;
-
-			/* magnetometer */
-			float mga2ga = 1.0e-3f;
-			hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
-			hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
-			hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
-			hil_sensors.magnetometer_ga[0] = imu.xmag;
-			hil_sensors.magnetometer_ga[1] = imu.ymag;
-			hil_sensors.magnetometer_ga[2] = imu.zmag;
-			hil_sensors.magnetometer_range_ga = 32.7f; // int16
-			hil_sensors.magnetometer_mode = 0; // TODO what is this
-			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
-			hil_sensors.magnetometer_counter = hil_counter;
-
-			/* baro */
-			hil_sensors.baro_pres_mbar = imu.abs_pressure;
-			hil_sensors.baro_alt_meter = imu.pressure_alt;
-			hil_sensors.baro_temp_celcius = imu.temperature;
-			hil_sensors.baro_counter = hil_counter;
-
-			/* differential pressure */
-			hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
-			hil_sensors.differential_pressure_counter = hil_counter;
-
-			/* airspeed from differential pressure, ambient pressure and temp */
-			struct airspeed_s airspeed;
-
-			float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa);
-			// XXX need to fix this
-			float tas = ias;
-
-			airspeed.timestamp = hrt_absolute_time();
-			airspeed.indicated_airspeed_m_s = ias;
-			airspeed.true_airspeed_m_s = tas;
-
-			if (pub_hil_airspeed < 0) {
-				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+		/* this means that heartbeats alone won't be published to the radio status no more */
+		_radio_status_available = true;
+	}
+}
 
-			} else {
-				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
-			}
+void
+MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
+{
+	mavlink_manual_control_t man;
+	mavlink_msg_manual_control_decode(msg, &man);
 
-			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
 
-			/* individual sensor publications */
-			struct gyro_report gyro;
-			gyro.x_raw = imu.xgyro / mrad2rad;
-			gyro.y_raw = imu.ygyro / mrad2rad;
-			gyro.z_raw = imu.zgyro / mrad2rad;
-			gyro.x = imu.xgyro;
-			gyro.y = imu.ygyro;
-			gyro.z = imu.zgyro;
-			gyro.temperature = imu.temperature;
-			gyro.timestamp = hrt_absolute_time();
+	manual.timestamp = hrt_absolute_time();
+	manual.x = man.x / 1000.0f;
+	manual.y = man.y / 1000.0f;
+	manual.r = man.r / 1000.0f;
+	manual.z = man.z / 1000.0f;
 
-			if (pub_hil_gyro < 0) {
-				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+	warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
 
-			} else {
-				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
-			}
+	if (_manual_pub < 0) {
+		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
 
-			struct accel_report accel;
+	} else {
+		orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
+	}
+}
 
-			accel.x_raw = imu.xacc / mg2ms2;
+void
+MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
+{
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
+	if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
+		mavlink_heartbeat_t hb;
+		mavlink_msg_heartbeat_decode(msg, &hb);
 
-			accel.y_raw = imu.yacc / mg2ms2;
+		/* 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();
 
-			accel.z_raw = imu.zacc / mg2ms2;
+			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+			if (!_radio_status_available) {
 
-			accel.x = imu.xacc;
+				struct telemetry_status_s tstatus;
+				memset(&tstatus, 0, sizeof(tstatus));
 
-			accel.y = imu.yacc;
+				tstatus.timestamp = _telemetry_heartbeat_time;
+				tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
-			accel.z = imu.zacc;
+				if (_telemetry_status_pub < 0) {
+					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-			accel.temperature = imu.temperature;
+				} else {
+					orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
+				}
+			}
+		}
+	}
+}
 
-			accel.timestamp = hrt_absolute_time();
+void
+MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
+{
+	mavlink_request_data_stream_t req;
+	mavlink_msg_request_data_stream_decode(msg, &req);
 
-			if (pub_hil_accel < 0) {
-				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
+		float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
 
-			} else {
-				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+		MavlinkStream *stream;
+		LL_FOREACH(_mavlink->get_streams(), stream) {
+			if (req.req_stream_id == stream->get_id()) {
+				_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
 			}
+		}
+	}
+}
 
-			struct mag_report mag;
-
-			mag.x_raw = imu.xmag / mga2ga;
+void
+MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
+{
+	mavlink_hil_sensor_t imu;
+	mavlink_msg_hil_sensor_decode(msg, &imu);
 
-			mag.y_raw = imu.ymag / mga2ga;
+	uint64_t timestamp = hrt_absolute_time();
 
-			mag.z_raw = imu.zmag / mga2ga;
+	/* airspeed */
+	{
+		struct airspeed_s airspeed;
+		memset(&airspeed, 0, sizeof(airspeed));
 
-			mag.x = imu.xmag;
+		float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
+		// XXX need to fix this
+		float tas = ias;
 
-			mag.y = imu.ymag;
+		airspeed.timestamp = timestamp;
+		airspeed.indicated_airspeed_m_s = ias;
+		airspeed.true_airspeed_m_s = tas;
 
-			mag.z = imu.zmag;
+		if (_airspeed_pub < 0) {
+			_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
 
-			mag.timestamp = hrt_absolute_time();
+		} else {
+			orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed);
+		}
+	}
 
-			if (pub_hil_mag < 0) {
-				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+	/* gyro */
+	{
+		struct gyro_report gyro;
+		memset(&gyro, 0, sizeof(gyro));
 
-			} else {
-				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
-			}
+		gyro.timestamp = timestamp;
+		gyro.x_raw = imu.xgyro * 1000.0f;
+		gyro.y_raw = imu.ygyro * 1000.0f;
+		gyro.z_raw = imu.zgyro * 1000.0f;
+		gyro.x = imu.xgyro;
+		gyro.y = imu.ygyro;
+		gyro.z = imu.zgyro;
+		gyro.temperature = imu.temperature;
 
-			struct baro_report baro;
+		if (_gyro_pub < 0) {
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
 
-			baro.pressure = imu.abs_pressure;
+		} else {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
+		}
+	}
 
-			baro.altitude = imu.pressure_alt;
+	/* accelerometer */
+	{
+		struct accel_report accel;
+		memset(&accel, 0, sizeof(accel));
 
-			baro.temperature = imu.temperature;
+		accel.timestamp = timestamp;
+		accel.x_raw = imu.xacc / mg2ms2;
+		accel.y_raw = imu.yacc / mg2ms2;
+		accel.z_raw = imu.zacc / mg2ms2;
+		accel.x = imu.xacc;
+		accel.y = imu.yacc;
+		accel.z = imu.zacc;
+		accel.temperature = imu.temperature;
 
-			baro.timestamp = hrt_absolute_time();
+		if (_accel_pub < 0) {
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
-			if (pub_hil_baro < 0) {
-				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+		} else {
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+		}
+	}
 
-			} else {
-				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
-			}
+	/* magnetometer */
+	{
+		struct mag_report mag;
+		memset(&mag, 0, sizeof(mag));
 
-			/* publish combined sensor topic */
-			if (pub_hil_sensors > 0) {
-				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+		mag.timestamp = timestamp;
+		mag.x_raw = imu.xmag * 1000.0f;
+		mag.y_raw = imu.ymag * 1000.0f;
+		mag.z_raw = imu.zmag * 1000.0f;
+		mag.x = imu.xmag;
+		mag.y = imu.ymag;
+		mag.z = imu.zmag;
 
-			} else {
-				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
-			}
+		if (_mag_pub < 0) {
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
 
-			/* fill in HIL battery status */
-			hil_battery_status.timestamp = hrt_absolute_time();
-			hil_battery_status.voltage_v = 11.1f;
-			hil_battery_status.current_a = 10.0f;
+		} else {
+			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
+		}
+	}
 
-			/* lazily publish the battery voltage */
-			if (pub_hil_battery > 0) {
-				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+	/* baro */
+	{
+		struct baro_report baro;
+		memset(&baro, 0, sizeof(baro));
 
-			} else {
-				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
-			}
+		baro.timestamp = timestamp;
+		baro.pressure = imu.abs_pressure;
+		baro.altitude = imu.pressure_alt;
+		baro.temperature = imu.temperature;
 
-			// increment counters
-			hil_counter++;
-			hil_frames++;
+		if (_baro_pub < 0) {
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
 
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil sensor at %d hz\n", hil_frames / 10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
+		} else {
+			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
 		}
+	}
 
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) {
-
-			mavlink_hil_gps_t gps;
-			mavlink_msg_hil_gps_decode(msg, &gps);
-
-			/* gps */
-			hil_gps.timestamp_position = gps.time_usec;
-			hil_gps.time_gps_usec = gps.time_usec;
-			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.s_variance_m_s = 5.0f;
-			hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
-			hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
-
-			/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
-			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
-
-			/* go back to -PI..PI */
-			if (heading_rad > M_PI_F)
-				heading_rad -= 2.0f * M_PI_F;
-
-			hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
-			hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
-			hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
-			hil_gps.vel_ned_valid = true;
-			/* COG (course over ground) is spec'ed as -PI..+PI */
-			hil_gps.cog_rad = heading_rad;
-			hil_gps.fix_type = gps.fix_type;
-			hil_gps.satellites_visible = gps.satellites_visible;
-
-			/* publish GPS measurement data */
-			if (pub_hil_gps > 0) {
-				orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
-
-			} else {
-				pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
-			}
+	/* sensor combined */
+	{
+		struct sensor_combined_s hil_sensors;
+		memset(&hil_sensors, 0, sizeof(hil_sensors));
+
+		hil_sensors.timestamp = timestamp;
+
+		hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
+		hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
+		hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
+		hil_sensors.gyro_rad_s[0] = imu.xgyro;
+		hil_sensors.gyro_rad_s[1] = imu.ygyro;
+		hil_sensors.gyro_rad_s[2] = imu.zgyro;
+
+		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
+		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
+		hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
+		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
+		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
+		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
+		hil_sensors.accelerometer_mode = 0; // TODO what is this?
+		hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
+		hil_sensors.accelerometer_timestamp = timestamp;
+
+		hil_sensors.adc_voltage_v[0] = 0.0f;
+		hil_sensors.adc_voltage_v[1] = 0.0f;
+		hil_sensors.adc_voltage_v[2] = 0.0f;
+
+		hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f;
+		hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f;
+		hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f;
+		hil_sensors.magnetometer_ga[0] = imu.xmag;
+		hil_sensors.magnetometer_ga[1] = imu.ymag;
+		hil_sensors.magnetometer_ga[2] = imu.zmag;
+		hil_sensors.magnetometer_range_ga = 32.7f; // int16
+		hil_sensors.magnetometer_mode = 0; // TODO what is this
+		hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
+		hil_sensors.magnetometer_timestamp = timestamp;
+
+		hil_sensors.baro_pres_mbar = imu.abs_pressure;
+		hil_sensors.baro_alt_meter = imu.pressure_alt;
+		hil_sensors.baro_temp_celcius = imu.temperature;
+		hil_sensors.baro_timestamp = timestamp;
+
+		hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
+		hil_sensors.differential_pressure_timestamp = timestamp;
+
+		/* publish combined sensor topic */
+		if (_sensors_pub < 0) {
+			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 
+		} else {
+			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
 		}
+	}
 
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) {
-
-			mavlink_hil_state_quaternion_t hil_state;
-			mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
-
-			struct airspeed_s airspeed;
-			airspeed.timestamp = hrt_absolute_time();
-			airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
-			airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
-
-			if (pub_hil_airspeed < 0) {
-				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+	/* battery status */
+	{
+		struct battery_status_s hil_battery_status;
+		memset(&hil_battery_status, 0, sizeof(hil_battery_status));
 
-			} else {
-				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
-			}
+		hil_battery_status.timestamp = timestamp;
+		hil_battery_status.voltage_v = 11.1f;
+		hil_battery_status.voltage_filtered_v = 11.1f;
+		hil_battery_status.current_a = 10.0f;
+		hil_battery_status.discharged_mah = -1.0f;
 
-			uint64_t timestamp = hrt_absolute_time();
-
-			// publish global position	
-			if (pub_hil_global_pos > 0) {
-				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
-				// global position packet
-				hil_global_pos.timestamp = timestamp;
-				hil_global_pos.valid = true;
-				hil_global_pos.lat = hil_state.lat;
-				hil_global_pos.lon = hil_state.lon;
-				hil_global_pos.alt = hil_state.alt / 1000.0f;
-				hil_global_pos.vx = hil_state.vx / 100.0f;
-				hil_global_pos.vy = hil_state.vy / 100.0f;
-				hil_global_pos.vz = hil_state.vz / 100.0f;
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
-			} else {
-				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
-			}
-
-			// publish local position
-			if (pub_hil_local_pos > 0) {
-				float x;
-				float y;
-				bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
-				double lat = hil_state.lat*1e-7;
-				double lon = hil_state.lon*1e-7;
-				map_projection_project(lat, lon, &x, &y); 
-				hil_local_pos.timestamp = timestamp;
-				hil_local_pos.xy_valid = true;
-				hil_local_pos.z_valid = true;
-				hil_local_pos.v_xy_valid = true;
-				hil_local_pos.v_z_valid = true;
-				hil_local_pos.x = x;
-				hil_local_pos.y = y;
-				hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
-				hil_local_pos.vx = hil_state.vx/100.0f;
-				hil_local_pos.vy = hil_state.vy/100.0f;
-				hil_local_pos.vz = hil_state.vz/100.0f;
-				hil_local_pos.yaw = hil_attitude.yaw;
-				hil_local_pos.xy_global = true;
-				hil_local_pos.z_global = true;
-				hil_local_pos.ref_timestamp = timestamp;
-				hil_local_pos.ref_lat = hil_state.lat;
-				hil_local_pos.ref_lon = hil_state.lon;
-				hil_local_pos.ref_alt = alt0;
-				hil_local_pos.landed = landed;
-				orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
-			} else {
-				pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
-				lat0 = hil_state.lat;
-				lon0 = hil_state.lon;
-				alt0 = hil_state.alt / 1000.0f;
-				map_projection_init(hil_state.lat, hil_state.lon);
-			}
+		} else {
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		}
+	}
 
-			/* Calculate Rotation Matrix */
-			math::Quaternion q(hil_state.attitude_quaternion);
-			math::Dcm C_nb(q);
-			math::EulerAngles euler(C_nb);
+	/* increment counters */
+	_hil_frames++;
 
-			/* set rotation matrix */
-			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-					hil_attitude.R[i][j] = C_nb(i, j);
+	/* print HIL sensors rate */
+	if ((timestamp - _old_timestamp) > 10000000) {
+		printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
+		_old_timestamp = timestamp;
+		_hil_frames = 0;
+	}
+}
 
-			hil_attitude.R_valid = true;
+void
+MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
+{
+	mavlink_hil_gps_t gps;
+	mavlink_msg_hil_gps_decode(msg, &gps);
 
-			/* set quaternion */
-			hil_attitude.q[0] = q(0);
-			hil_attitude.q[1] = q(1);
-			hil_attitude.q[2] = q(2);
-			hil_attitude.q[3] = q(3);
-			hil_attitude.q_valid = true;
+	uint64_t timestamp = hrt_absolute_time();
 
-			hil_attitude.roll = euler.getPhi();
-			hil_attitude.pitch = euler.getTheta();
-			hil_attitude.yaw = euler.getPsi();
-			hil_attitude.rollspeed = hil_state.rollspeed;
-			hil_attitude.pitchspeed = hil_state.pitchspeed;
-			hil_attitude.yawspeed = hil_state.yawspeed;
+	struct vehicle_gps_position_s hil_gps;
+	memset(&hil_gps, 0, sizeof(hil_gps));
 
-			/* set timestamp and notify processes (broadcast) */
-			hil_attitude.timestamp = hrt_absolute_time();
+	hil_gps.timestamp_time = timestamp;
+	hil_gps.time_gps_usec = gps.time_usec;
 
-			if (pub_hil_attitude > 0) {
-				orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+	hil_gps.timestamp_position = timestamp;
+	hil_gps.lat = gps.lat;
+	hil_gps.lon = gps.lon;
+	hil_gps.alt = gps.alt;
+	hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
+	hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
 
-			} else {
-				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
-			}
+	hil_gps.timestamp_variance = timestamp;
+	hil_gps.s_variance_m_s = 5.0f;
 
-			struct accel_report accel;
+	hil_gps.timestamp_velocity = timestamp;
+	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
+	hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
+	hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
+	hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
+	hil_gps.vel_ned_valid = true;
+	hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
 
-			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+	hil_gps.fix_type = gps.fix_type;
+	hil_gps.satellites_used = gps.satellites_visible;  //TODO: rename mavlink_hil_gps_t sats visible to used?
 
-			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+	if (_gps_pub < 0) {
+		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 
-			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
-
-			accel.x = hil_state.xacc;
-
-			accel.y = hil_state.yacc;
+	} else {
+		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
+	}
+}
 
-			accel.z = hil_state.zacc;
+void
+MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
+{
+	mavlink_hil_state_quaternion_t hil_state;
+	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
-			accel.temperature = 25.0f;
+	uint64_t timestamp = hrt_absolute_time();
 
-			accel.timestamp = hrt_absolute_time();
+	/* airspeed */
+	{
+		struct airspeed_s airspeed;
+		memset(&airspeed, 0, sizeof(airspeed));
 
-			if (pub_hil_accel < 0) {
-				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+		airspeed.timestamp = timestamp;
+		airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
+		airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
 
-			} else {
-				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
-			}
+		if (_airspeed_pub < 0) {
+			_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
 
-			/* fill in HIL battery status */
-			hil_battery_status.timestamp = hrt_absolute_time();
-			hil_battery_status.voltage_v = 11.1f;
-			hil_battery_status.current_a = 10.0f;
+		} else {
+			orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed);
+		}
+	}
 
-			/* lazily publish the battery voltage */
-			if (pub_hil_battery > 0) {
-				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+	/* attitude */
+	struct vehicle_attitude_s hil_attitude;
+	{
+		memset(&hil_attitude, 0, sizeof(hil_attitude));
+		math::Quaternion q(hil_state.attitude_quaternion);
+		math::Matrix<3, 3> C_nb = q.to_dcm();
+		math::Vector<3> euler = C_nb.to_euler();
+
+		hil_attitude.timestamp = timestamp;
+		memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
+		hil_attitude.R_valid = true;
+
+		hil_attitude.q[0] = q(0);
+		hil_attitude.q[1] = q(1);
+		hil_attitude.q[2] = q(2);
+		hil_attitude.q[3] = q(3);
+		hil_attitude.q_valid = true;
+
+		hil_attitude.roll = euler(0);
+		hil_attitude.pitch = euler(1);
+		hil_attitude.yaw = euler(2);
+		hil_attitude.rollspeed = hil_state.rollspeed;
+		hil_attitude.pitchspeed = hil_state.pitchspeed;
+		hil_attitude.yawspeed = hil_state.yawspeed;
+
+		if (_attitude_pub < 0) {
+			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 
-			} else {
-				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
-			}
+		} else {
+			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
 		}
+	}
 
-		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
-			mavlink_manual_control_t man;
-			mavlink_msg_manual_control_decode(msg, &man);
-
-			struct rc_channels_s rc_hil;
-			memset(&rc_hil, 0, sizeof(rc_hil));
-			static orb_advert_t rc_pub = 0;
+	/* global position */
+	{
+		struct vehicle_global_position_s hil_global_pos;
+		memset(&hil_global_pos, 0, sizeof(hil_global_pos));
+
+		hil_global_pos.timestamp = timestamp;
+		hil_global_pos.lat = hil_state.lat;
+		hil_global_pos.lon = hil_state.lon;
+		hil_global_pos.alt = hil_state.alt / 1000.0f;
+		hil_global_pos.vel_n = hil_state.vx / 100.0f;
+		hil_global_pos.vel_e = hil_state.vy / 100.0f;
+		hil_global_pos.vel_d = hil_state.vz / 100.0f;
+		hil_global_pos.yaw = hil_attitude.yaw;
+		hil_global_pos.eph = 2.0f;
+		hil_global_pos.epv = 4.0f;
+
+		if (_global_pos_pub < 0) {
+			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 
-			rc_hil.timestamp = hrt_absolute_time();
-			rc_hil.chan_count = 4;
+		} else {
+			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
+		}
+	}
 
-			rc_hil.chan[0].scaled = man.x / 1000.0f;
-			rc_hil.chan[1].scaled = man.y / 1000.0f;
-			rc_hil.chan[2].scaled = man.r / 1000.0f;
-			rc_hil.chan[3].scaled = man.z / 1000.0f;
+	/* local position */
+	{
+		double lat = hil_state.lat * 1e-7;
+		double lon = hil_state.lon * 1e-7;
+
+		if (!_hil_local_proj_inited) {
+			_hil_local_proj_inited = true;
+			_hil_local_alt0 = hil_state.alt / 1000.0f;
+			map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
+			hil_local_pos.ref_timestamp = timestamp;
+			hil_local_pos.ref_lat = lat;
+			hil_local_pos.ref_lon = lon;
+			hil_local_pos.ref_alt = _hil_local_alt0;
+		}
 
-			struct manual_control_setpoint_s mc;
-			static orb_advert_t mc_pub = 0;
+		float x;
+		float y;
+		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
+		hil_local_pos.timestamp = timestamp;
+		hil_local_pos.xy_valid = true;
+		hil_local_pos.z_valid = true;
+		hil_local_pos.v_xy_valid = true;
+		hil_local_pos.v_z_valid = true;
+		hil_local_pos.x = x;
+		hil_local_pos.y = y;
+		hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f;
+		hil_local_pos.vx = hil_state.vx / 100.0f;
+		hil_local_pos.vy = hil_state.vy / 100.0f;
+		hil_local_pos.vz = hil_state.vz / 100.0f;
+		hil_local_pos.yaw = hil_attitude.yaw;
+		hil_local_pos.xy_global = true;
+		hil_local_pos.z_global = true;
+
+		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
+		hil_local_pos.landed = landed;
+
+		if (_local_pos_pub < 0) {
+			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
 
-			int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+		} else {
+			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
+		}
+	}
 
-			/* get a copy first, to prevent altering values that are not sent by the mavlink command */
-			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
+	/* accelerometer */
+	{
+		struct accel_report accel;
+		memset(&accel, 0, sizeof(accel));
 
-			mc.timestamp = rc_hil.timestamp;
-			mc.roll = man.x / 1000.0f;
-			mc.pitch = man.y / 1000.0f;
-			mc.yaw = man.r / 1000.0f;
-			mc.throttle = man.z / 1000.0f;
+		accel.timestamp = timestamp;
+		accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
+		accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f;
+		accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f;
+		accel.x = hil_state.xacc;
+		accel.y = hil_state.yacc;
+		accel.z = hil_state.zacc;
+		accel.temperature = 25.0f;
 
-			/* fake RC channels with manual control input from simulator */
+		if (_accel_pub < 0) {
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
+		} else {
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+		}
+	}
 
-			if (rc_pub == 0) {
-				rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
+	/* battery status */
+	{
+		struct battery_status_s	hil_battery_status;
+		memset(&hil_battery_status, 0, sizeof(hil_battery_status));
 
-			} else {
-				orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
-			}
+		hil_battery_status.timestamp = timestamp;
+		hil_battery_status.voltage_v = 11.1f;
+		hil_battery_status.voltage_filtered_v = 11.1f;
+		hil_battery_status.current_a = 10.0f;
+		hil_battery_status.discharged_mah = -1.0f;
 
-			if (mc_pub == 0) {
-				mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
-			} else {
-				orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
-			}
+		} else {
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 }
@@ -809,17 +915,20 @@ handle_message(mavlink_message_t *msg)
 /**
  * Receive data from UART.
  */
-static void *
-receive_thread(void *arg)
+void *
+MavlinkReceiver::receive_thread(void *arg)
 {
-	int uart_fd = *((int *)arg);
+	int uart_fd = _mavlink->get_uart_fd();
 
-	const int timeout = 1000;
+	const int timeout = 500;
 	uint8_t buf[32];
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid());
+	/* set thread name */
+	char thread_name[24];
+	sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
+	prctl(PR_SET_NAME, thread_name, getpid());
 
 	struct pollfd fds[1];
 	fds[0].fd = uart_fd;
@@ -827,27 +936,23 @@ receive_thread(void *arg)
 
 	ssize_t nread = 0;
 
-	while (!thread_should_exit) {
+	while (!_mavlink->_task_should_exit) {
 		if (poll(fds, 1, timeout) > 0) {
-			if (nread < sizeof(buf)) {
+
+			/* non-blocking read. read may return negative values */
+			if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
 				/* to avoid reading very small chunks wait for data before reading */
 				usleep(1000);
 			}
 
-			/* non-blocking read. read may return negative values */
-			nread = read(uart_fd, buf, sizeof(buf));
-
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
-				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
+				if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
-					/* handle packet with waypoint component */
-					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
-
-					/* handle packet with parameter component */
-					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
+					/* handle packet with parent object */
+					_mavlink->handle_message(&msg);
 				}
 			}
 		}
@@ -856,25 +961,40 @@ receive_thread(void *arg)
 	return NULL;
 }
 
+void MavlinkReceiver::print_status()
+{
+
+}
+
+void *MavlinkReceiver::start_helper(void *context)
+{
+	MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);
+
+	rcv->receive_thread(NULL);
+
+	delete rcv;
+
+	return nullptr;
+}
+
 pthread_t
-receive_start(int uart)
+MavlinkReceiver::receive_start(Mavlink *parent)
 {
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 
 	// set to non-blocking read
-	int flags = fcntl(uart, F_GETFL, 0);
-	fcntl(uart, F_SETFL, flags | O_NONBLOCK);
+	int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0);
+	fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
 	param.sched_priority = SCHED_PRIORITY_MAX - 40;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 3000);
-
+	pthread_attr_setstacksize(&receiveloop_attr, 2900);
 	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 
 	pthread_attr_destroy(&receiveloop_attr);
 	return thread;
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/mavlink_receiver.h
index 91773843b..8d38b9973 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.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
@@ -33,29 +32,34 @@
  ****************************************************************************/
 
 /**
- * @file orb_topics.h
- * Common sets of topics subscribed to or published by the MAVLink driver,
- * and structures maintained by those subscriptions.
+ * @file mavlink_orb_listener.h
+ * MAVLink 1.0 uORB listener definition
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
+
 #pragma once
 
+#include <systemlib/perf_counter.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls_effective.h>
@@ -66,55 +70,90 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
-#include <drivers/drv_rc_input.h>
-#include <uORB/topics/navigation_capabilities.h>
 
-struct mavlink_subscriptions {
-	int sensor_sub;
-	int att_sub;
-	int global_pos_sub;
-	int act_0_sub;
-	int act_1_sub;
-	int act_2_sub;
-	int act_3_sub;
-	int gps_sub;
-	int man_control_sp_sub;
-	int safety_sub;
-	int actuators_sub;
-	int armed_sub;
-	int actuators_effective_sub;
-	int local_pos_sub;
-	int spa_sub;
-	int spl_sub;
-	int spg_sub;
-	int debug_key_value;
-	int input_rc_sub;
-	int optical_flow;
-	int rates_setpoint_sub;
-	int home_sub;
-	int airspeed_sub;
-	int navigation_capabilities_sub;
-};
+#include "mavlink_ftp.h"
 
-extern struct mavlink_subscriptions mavlink_subs;
+class Mavlink;
 
-/** Global position */
-extern struct vehicle_global_position_s global_pos;
+class MavlinkReceiver
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MavlinkReceiver(Mavlink *parent);
 
-/** Local position */
-extern struct vehicle_local_position_s local_pos;
+	/**
+	 * Destructor, also kills the mavlinks task.
+	 */
+	~MavlinkReceiver();
 
-/** navigation capabilities */
-extern struct navigation_capabilities_s nav_cap;
+	/**
+	* Start the mavlink task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
 
-/** Vehicle status */
-extern struct vehicle_status_s v_status;
+	/**
+	 * Display the mavlink status.
+	 */
+	void		print_status();
 
-/** RC channels */
-extern struct rc_channels_s rc;
+	static pthread_t receive_start(Mavlink *parent);
 
-/** Actuator armed state */
-extern struct actuator_armed_s armed;
+	static void *start_helper(void *context);
 
-/** Worker thread starter */
-extern pthread_t uorb_receive_start(void);
+private:
+	Mavlink	*_mavlink;
+
+	void handle_message(mavlink_message_t *msg);
+	void handle_message_command_long(mavlink_message_t *msg);
+	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_set_mode(mavlink_message_t *msg);
+	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
+	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_request_data_stream(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);
+
+	void *receive_thread(void *arg);
+
+	mavlink_status_t status;
+	struct vehicle_local_position_s hil_local_pos;
+	struct vehicle_control_mode_s _control_mode;
+	orb_advert_t _global_pos_pub;
+	orb_advert_t _local_pos_pub;
+	orb_advert_t _attitude_pub;
+	orb_advert_t _gps_pub;
+	orb_advert_t _sensors_pub;
+	orb_advert_t _gyro_pub;
+	orb_advert_t _accel_pub;
+	orb_advert_t _mag_pub;
+	orb_advert_t _baro_pub;
+	orb_advert_t _airspeed_pub;
+	orb_advert_t _battery_pub;
+	orb_advert_t _cmd_pub;
+	orb_advert_t _flow_pub;
+	orb_advert_t _offboard_control_sp_pub;
+	orb_advert_t _local_pos_sp_pub;
+	orb_advert_t _global_vel_sp_pub;
+	orb_advert_t _att_sp_pub;
+	orb_advert_t _rates_sp_pub;
+	orb_advert_t _vicon_position_pub;
+	orb_advert_t _telemetry_status_pub;
+	orb_advert_t _rc_pub;
+	orb_advert_t _manual_pub;
+	hrt_abstime _telemetry_heartbeat_time;
+	bool _radio_status_available;
+	int _control_mode_sub;
+	int _hil_frames;
+	uint64_t _old_timestamp;
+	bool _hil_local_proj_inited;
+	float _hil_local_alt0;
+	struct map_projection_reference_s _hil_local_proj_ref;
+};
diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 68e741817..7279206db 100644
--- a/src/lib/mathlib/math/Vector2f.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,72 +32,62 @@
  ****************************************************************************/
 
 /**
- * @file Vector2f.cpp
+ * @file mavlink_stream.cpp
+ * Mavlink messages stream implementation.
  *
- * math vector
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include "test/test.hpp"
+#include <stdlib.h>
 
-#include "Vector2f.hpp"
+#include "mavlink_stream.h"
+#include "mavlink_main.h"
 
-namespace math
-{
-
-Vector2f::Vector2f() :
-	Vector(2)
+MavlinkStream::MavlinkStream() :
+	next(nullptr),
+	_channel(MAVLINK_COMM_0),
+	_interval(1000000),
+	_last_sent(0)
 {
 }
 
-Vector2f::Vector2f(const Vector &right) :
-	Vector(right)
+MavlinkStream::~MavlinkStream()
 {
-#ifdef VECTOR_ASSERT
-	ASSERT(right.getRows() == 2);
-#endif
 }
 
-Vector2f::Vector2f(float x, float y) :
-	Vector(2)
+/**
+ * Set messages interval in ms
+ */
+void
+MavlinkStream::set_interval(const unsigned int interval)
 {
-	setX(x);
-	setY(y);
+	_interval = interval;
 }
 
-Vector2f::Vector2f(const float *data) :
-	Vector(2, data)
+/**
+ * Set mavlink channel
+ */
+void
+MavlinkStream::set_channel(mavlink_channel_t channel)
 {
+	_channel = channel;
 }
 
-Vector2f::~Vector2f()
+/**
+ * Update subscriptions and send message if necessary
+ */
+int
+MavlinkStream::update(const hrt_abstime t)
 {
-}
+	uint64_t dt = t - _last_sent;
 
-float Vector2f::cross(const Vector2f &b) const
-{
-	const Vector2f &a = *this;
-	return a(0)*b(1) - a(1)*b(0);
-}
+	if (dt > 0 && dt >= _interval) {
+		/* interval expired, send message */
+		send(t);
+		_last_sent = (t / _interval) * _interval;
 
-float Vector2f::operator %(const Vector2f &v) const
-{
-	return cross(v);
-}
-    
-float Vector2f::operator *(const Vector2f &v) const
-{
-    return dot(v);
-}
+		return 0;
+	}
 
-int __EXPORT vector2fTest()
-{
-	printf("Test Vector2f\t\t: ");
-	// test float ctor
-	Vector2f v(1, 2);
-	ASSERT(equal(v(0), 1));
-	ASSERT(equal(v(1), 2));
-	printf("PASS\n");
-	return 0;
+	return -1;
 }
-
-} // namespace math
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
new file mode 100644
index 000000000..69809a386
--- /dev/null
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   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_stream.h
+ * Mavlink messages stream definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_STREAM_H_
+#define MAVLINK_STREAM_H_
+
+#include <drivers/drv_hrt.h>
+
+class Mavlink;
+class MavlinkStream;
+
+#include "mavlink_main.h"
+
+class MavlinkStream
+{
+
+public:
+	MavlinkStream *next;
+
+	MavlinkStream();
+	virtual ~MavlinkStream();
+	void set_interval(const unsigned int interval);
+	void set_channel(mavlink_channel_t channel);
+
+	/**
+	 * @return 0 if updated / sent, -1 if unchanged
+	 */
+	int update(const hrt_abstime t);
+	static MavlinkStream *new_instance();
+	static const char *get_name_static();
+	virtual void subscribe(Mavlink *mavlink) = 0;
+	virtual const char *get_name() const = 0;
+	virtual uint8_t get_id() = 0;
+
+protected:
+	mavlink_channel_t _channel;
+	unsigned int _interval;
+
+	virtual void send(const hrt_abstime t) = 0;
+
+private:
+	hrt_abstime _last_sent;
+};
+
+
+#endif /* MAVLINK_STREAM_H_ */
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
deleted file mode 100644
index 124b3b2ae..000000000
--- a/src/modules/mavlink/missionlib.c
+++ /dev/null
@@ -1,390 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @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
- * 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 missionlib.h
- * MAVLink missionlib components
- */
-
-// XXX trim includes
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
-#include <systemlib/err.h>
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <mavlink/mavlink_log.h>
-
-#include "geo/geo.h"
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "util.h"
-#include "waypoints.h"
-#include "mavlink_parameters.h"
-
-static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
-static uint64_t loiter_start_time;
-
-static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
-	struct vehicle_global_position_setpoint_s *sp);
-
-int
-mavlink_missionlib_send_message(mavlink_message_t *msg)
-{
-	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
-
-	mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
-	return 0;
-}
-
-int
-mavlink_missionlib_send_gcs_string(const char *string)
-{
-	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
-	mavlink_statustext_t statustext;
-	int i = 0;
-
-	while (i < len - 1) {
-		statustext.text[i] = string[i];
-
-		if (string[i] == '\0')
-			break;
-
-		i++;
-	}
-
-	if (i > 1) {
-		/* Enforce null termination */
-		statustext.text[i] = '\0';
-		mavlink_message_t msg;
-
-		mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
-		return mavlink_missionlib_send_message(&msg);
-
-	} else {
-		return 1;
-	}
-}
-
-/**
- * Get system time since boot in microseconds
- *
- * @return the system time since boot in microseconds
- */
-uint64_t mavlink_missionlib_get_system_timestamp()
-{
-	return hrt_absolute_time();
-}
-
-/**
- * Set special vehicle setpoint fields based on current mission item.
- *
- * @return true if the mission item could be interpreted
- * successfully, it return false on failure.
- */
-bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
-	struct vehicle_global_position_setpoint_s *sp)
-{
-	switch (command) {
-		case MAV_CMD_NAV_LOITER_UNLIM:
-			sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
-			break;
-		case MAV_CMD_NAV_LOITER_TIME:
-			sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-			loiter_start_time = hrt_absolute_time();
-			break;
-		// case MAV_CMD_NAV_LOITER_TURNS:
-		// 	sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT;
-		// 	break;
-		case MAV_CMD_NAV_WAYPOINT:
-			sp->nav_cmd = NAV_CMD_WAYPOINT;
-			break;
-		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-			sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
-			break;
-		case MAV_CMD_NAV_LAND:
-			sp->nav_cmd = NAV_CMD_LAND;
-			break;
-		case MAV_CMD_NAV_TAKEOFF:
-			sp->nav_cmd = NAV_CMD_TAKEOFF;
-			break;
-		default:
-			/* abort */
-			return false;
-	}
-
-	sp->loiter_radius = param3;
-	sp->loiter_direction = (param3 >= 0) ? 1 : -1;
-
-	sp->param1 = param1;
-	sp->param2 = param2;
-	sp->param3 = param3;
-	sp->param4 = param4;
-
-
-	/* define the turn distance */
-	float orbit = 15.0f;
-
-	if (command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-		orbit = param2;
-
-	} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-			command == (int)MAV_CMD_NAV_LOITER_TIME ||
-			command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-		orbit = param3;
-	} else {
-
-		// XXX set default orbit via param
-		// 15 initialized above
-	}
-
-	sp->turn_distance_xy = orbit;
-	sp->turn_distance_z = orbit;
-}
-
-/**
- * This callback is executed each time a waypoint changes.
- *
- * It publishes the vehicle_global_position_setpoint_s or the
- * vehicle_local_position_setpoint_s topic, depending on the type of waypoint
- */
-void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
-{
-	static orb_advert_t global_position_setpoint_pub = -1;
-	static orb_advert_t global_position_set_triplet_pub = -1;
-	static orb_advert_t local_position_setpoint_pub = -1;
-	static unsigned last_waypoint_index = -1;
-	char buf[50] = {0};
-
-	// XXX include check if WP is supported, jump to next if not
-
-	/* Update controller setpoints */
-	if (frame == (int)MAV_FRAME_GLOBAL) {
-		/* global, absolute waypoint */
-		struct vehicle_global_position_setpoint_s sp;
-		sp.lat = param5_lat_x * 1e7f;
-		sp.lon = param6_lon_y * 1e7f;
-		sp.altitude = param7_alt_z;
-		sp.altitude_is_relative = false;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-		set_special_fields(param1, param2, param3, param4, command, &sp);
-
-		/* Initialize setpoint publication if necessary */
-		if (global_position_setpoint_pub < 0) {
-			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
-		}
-
-
-		/* fill triplet: previous, current, next waypoint */
-		struct vehicle_global_position_set_triplet_s triplet;
-
-		/* current waypoint is same as sp */
-		memcpy(&(triplet.current), &sp, sizeof(sp));
-
-		/*
-		 * Check if previous WP (in mission, not in execution order) 
-		 * is available and identify correct index
-		 */
-		int last_setpoint_index = -1;
-		bool last_setpoint_valid = false;
-
-		if (index > 0) {
-			last_setpoint_index = index - 1;
-		}
-
-		while (last_setpoint_index >= 0) {
-
-			if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL &&
-				(wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
-				last_setpoint_valid = true;
-				break;
-			}
-
-			last_setpoint_index--;
-		}
-
-		/*
-		 * Check if next WP (in mission, not in execution order) 
-		 * is available and identify correct index
-		 */
-		int next_setpoint_index = -1;
-		bool next_setpoint_valid = false;
-
-		/* next waypoint */
-		if (wpm->size > 1) {
-			next_setpoint_index = index + 1;
-		}
-
-		while (next_setpoint_index < wpm->size) {
-
-			if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
-				next_setpoint_valid = true;
-				break;
-			}
-
-			next_setpoint_index++;
-		}
-
-		/* populate last and next */
-
-		triplet.previous_valid = false;
-		triplet.next_valid = false;
-
-		if (last_setpoint_valid) {
-			triplet.previous_valid = true;
-			struct vehicle_global_position_setpoint_s sp;
-			sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
-			sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
-			sp.altitude = wpm->waypoints[last_setpoint_index].z;
-			sp.altitude_is_relative = false;
-			sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
-			set_special_fields(wpm->waypoints[last_setpoint_index].param1,
-				wpm->waypoints[last_setpoint_index].param2,
-				wpm->waypoints[last_setpoint_index].param3,
-				wpm->waypoints[last_setpoint_index].param4,
-				wpm->waypoints[last_setpoint_index].command, &sp);
-			memcpy(&(triplet.previous), &sp, sizeof(sp));
-		}
-
-		if (next_setpoint_valid) {
-			triplet.next_valid = true;
-			struct vehicle_global_position_setpoint_s sp;
-			sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
-			sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
-			sp.altitude = wpm->waypoints[next_setpoint_index].z;
-			sp.altitude_is_relative = false;
-			sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
-			set_special_fields(wpm->waypoints[next_setpoint_index].param1,
-				wpm->waypoints[next_setpoint_index].param2,
-				wpm->waypoints[next_setpoint_index].param3,
-				wpm->waypoints[next_setpoint_index].param4,
-				wpm->waypoints[next_setpoint_index].command, &sp);
-			memcpy(&(triplet.next), &sp, sizeof(sp));
-		}
-
-		/* Initialize triplet publication if necessary */
-		if (global_position_set_triplet_pub < 0) {
-			global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
-		}
-
-		sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-
-	} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-		/* global, relative alt (in relation to HOME) waypoint */
-		struct vehicle_global_position_setpoint_s sp;
-		sp.lat = param5_lat_x * 1e7f;
-		sp.lon = param6_lon_y * 1e7f;
-		sp.altitude = param7_alt_z;
-		sp.altitude_is_relative = true;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-		set_special_fields(param1, param2, param3, param4, command, &sp);
-
-		/* Initialize publication if necessary */
-		if (global_position_setpoint_pub < 0) {
-			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
-		}
-
-
-
-		sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-
-	} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
-		/* local, absolute waypoint */
-		struct vehicle_local_position_setpoint_s sp;
-		sp.x = param5_lat_x;
-		sp.y = param6_lon_y;
-		sp.z = param7_alt_z;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-
-		/* Initialize publication if necessary */
-		if (local_position_setpoint_pub < 0) {
-			local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
-		}
-
-		sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-	} else {
-		warnx("non-navigation WP, ignoring");
-		mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring.");
-		return;
-	}
-
-	/* only set this for known waypoint types (non-navigation types would have returned earlier) */
-	last_waypoint_index = index;
-
-	mavlink_missionlib_send_gcs_string(buf);
-	printf("%s\n", buf);
-	//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
-}
diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h
deleted file mode 100644
index c7d8f90c5..000000000
--- a/src/modules/mavlink/missionlib.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @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
- * 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 missionlib.h
- * MAVLink mission helper library
- */
-
-#pragma once
-
-#include "mavlink_bridge_header.h"
-
-//extern void	mavlink_wpm_send_message(mavlink_message_t *msg);
-//extern void	mavlink_wpm_send_gcs_string(const char *string);
-//extern uint64_t	mavlink_wpm_get_system_timestamp(void);
-extern int	mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern int	mavlink_missionlib_send_gcs_string(const char *string);
-extern uint64_t	mavlink_missionlib_get_system_timestamp(void);
-extern void	mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 5d3d6a73c..d49bbb7f7 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#   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
@@ -36,11 +36,19 @@
 #
 
 MODULE_COMMAND	 = mavlink
-SRCS		 += mavlink.c \
-			   missionlib.c \
-			   mavlink_parameters.c \
-			   mavlink_receiver.cpp \
-			   orb_listener.c \
-			   waypoints.c
+SRCS		 += mavlink_main.cpp \
+			mavlink.c \
+			mavlink_receiver.cpp \
+			mavlink_mission.cpp \
+			mavlink_orb_subscription.cpp \
+			mavlink_messages.cpp \
+			mavlink_stream.cpp \
+			mavlink_rate_limiter.cpp \
+			mavlink_commands.cpp \
+			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1024
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
deleted file mode 100644
index e1dabfd21..000000000
--- a/src/modules/mavlink/orb_listener.c
+++ /dev/null
@@ -1,848 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file orb_listener.c
- * Monitors ORB topics and sends update messages as appropriate.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-// XXX trim includes
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <sys/prctl.h>
-#include <stdlib.h>
-#include <poll.h>
-#include <lib/geo/geo.h>
-
-#include <mavlink/mavlink_log.h>
-
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "util.h"
-
-extern bool gcs_link;
-
-struct vehicle_global_position_s global_pos;
-struct vehicle_local_position_s local_pos;
-struct navigation_capabilities_s nav_cap;
-struct vehicle_status_s v_status;
-struct rc_channels_s rc;
-struct rc_input_values rc_raw;
-struct actuator_armed_s armed;
-struct actuator_controls_s actuators_0;
-struct vehicle_attitude_s att;
-struct airspeed_s airspeed;
-
-struct mavlink_subscriptions mavlink_subs;
-
-static int status_sub;
-static int rc_sub;
-
-static unsigned int sensors_raw_counter;
-static unsigned int attitude_counter;
-static unsigned int gps_counter;
-
-/*
- * Last sensor loop time
- * some outputs are better timestamped
- * with this "global" reference.
- */
-static uint64_t last_sensor_timestamp;
-
-static hrt_abstime last_sent_vfr = 0;
-
-static void	*uorb_receive_thread(void *arg);
-
-struct listener {
-	void	(*callback)(const struct listener *l);
-	int		*subp;
-	uintptr_t	arg;
-};
-
-uint16_t cm_uint16_from_m_float(float m);
-
-static void	l_sensor_combined(const struct listener *l);
-static void	l_vehicle_attitude(const struct listener *l);
-static void	l_vehicle_gps_position(const struct listener *l);
-static void	l_vehicle_status(const struct listener *l);
-static void	l_rc_channels(const struct listener *l);
-static void	l_input_rc(const struct listener *l);
-static void	l_global_position(const struct listener *l);
-static void	l_local_position(const struct listener *l);
-static void	l_global_position_setpoint(const struct listener *l);
-static void	l_local_position_setpoint(const struct listener *l);
-static void	l_attitude_setpoint(const struct listener *l);
-static void	l_actuator_outputs(const struct listener *l);
-static void	l_actuator_armed(const struct listener *l);
-static void	l_manual_control_setpoint(const struct listener *l);
-static void	l_vehicle_attitude_controls(const struct listener *l);
-static void	l_debug_key_value(const struct listener *l);
-static void	l_optical_flow(const struct listener *l);
-static void	l_vehicle_rates_setpoint(const struct listener *l);
-static void	l_home(const struct listener *l);
-static void	l_airspeed(const struct listener *l);
-static void	l_nav_cap(const struct listener *l);
-
-static const struct listener listeners[] = {
-	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
-	{l_vehicle_attitude,		&mavlink_subs.att_sub,		0},
-	{l_vehicle_gps_position,	&mavlink_subs.gps_sub,		0},
-	{l_vehicle_status,		&status_sub,			0},
-	{l_rc_channels,			&rc_sub,			0},
-	{l_input_rc,			&mavlink_subs.input_rc_sub,	0},
-	{l_global_position,		&mavlink_subs.global_pos_sub,	0},
-	{l_local_position,		&mavlink_subs.local_pos_sub,	0},
-	{l_global_position_setpoint,	&mavlink_subs.spg_sub,		0},
-	{l_local_position_setpoint,	&mavlink_subs.spl_sub,		0},
-	{l_attitude_setpoint,		&mavlink_subs.spa_sub,		0},
-	{l_actuator_outputs,		&mavlink_subs.act_0_sub,	0},
-	{l_actuator_outputs,		&mavlink_subs.act_1_sub,	1},
-	{l_actuator_outputs,		&mavlink_subs.act_2_sub,	2},
-	{l_actuator_outputs,		&mavlink_subs.act_3_sub,	3},
-	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
-	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
-	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
-	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
-	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
-	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
-	{l_home,			&mavlink_subs.home_sub,		0},
-	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
-	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
-};
-
-static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
-
-uint16_t
-cm_uint16_from_m_float(float m)
-{
-	if (m < 0.0f) {
-		return 0;
-
-	} else if (m > 655.35f) {
-		return 65535;
-	}
-
-	return (uint16_t)(m * 100.0f);
-}
-
-void
-l_sensor_combined(const struct listener *l)
-{
-	struct sensor_combined_s raw;
-
-	/* copy sensors raw data into local buffer */
-	orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw);
-
-	last_sensor_timestamp = raw.timestamp;
-
-	/* mark individual fields as changed */
-	uint16_t fields_updated = 0;
-	static unsigned accel_counter = 0;
-	static unsigned gyro_counter = 0;
-	static unsigned mag_counter = 0;
-	static unsigned baro_counter = 0;
-
-	if (accel_counter != raw.accelerometer_counter) {
-		/* mark first three dimensions as changed */
-		fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-		accel_counter = raw.accelerometer_counter;
-	}
-
-	if (gyro_counter != raw.gyro_counter) {
-		/* mark second group dimensions as changed */
-		fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-		gyro_counter = raw.gyro_counter;
-	}
-
-	if (mag_counter != raw.magnetometer_counter) {
-		/* mark third group dimensions as changed */
-		fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-		mag_counter = raw.magnetometer_counter;
-	}
-
-	if (baro_counter != raw.baro_counter) {
-		/* mark last group dimensions as changed */
-		fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-		baro_counter = raw.baro_counter;
-	}
-
-	if (gcs_link)
-		mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
-					     raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
-					     raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
-					     raw.gyro_rad_s[1], raw.gyro_rad_s[2],
-					     raw.magnetometer_ga[0],
-					     raw.magnetometer_ga[1], raw.magnetometer_ga[2],
-					     raw.baro_pres_mbar, raw.differential_pressure_pa,
-					     raw.baro_alt_meter, raw.baro_temp_celcius,
-					     fields_updated);
-
-	sensors_raw_counter++;
-}
-
-void
-l_vehicle_attitude(const struct listener *l)
-{
-	/* copy attitude data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
-
-	if (gcs_link) {
-		/* send sensor values */
-		mavlink_msg_attitude_send(MAVLINK_COMM_0,
-					  last_sensor_timestamp / 1000,
-					  att.roll,
-					  att.pitch,
-					  att.yaw,
-					  att.rollspeed,
-					  att.pitchspeed,
-					  att.yawspeed);
-					  	
-		/* limit VFR message rate to 10Hz */
-		hrt_abstime t = hrt_absolute_time();
-		if (t >= last_sent_vfr + 100000) {
-			last_sent_vfr = t;
-			float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
-			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
-			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
-		}
-		
-		/* send quaternion values if it exists */
-		if(att.q_valid) {
-			mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
-												last_sensor_timestamp / 1000,
-												att.q[0],
-												att.q[1],
-												att.q[2],
-												att.q[3],
-												att.rollspeed,
-												att.pitchspeed,
-												att.yawspeed);
-		}
-	}
-
-	attitude_counter++;
-}
-
-void
-l_vehicle_gps_position(const struct listener *l)
-{
-	struct vehicle_gps_position_s gps;
-
-	/* copy gps data into local buffer */
-	orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
-
-	/* GPS COG is 0..2PI in degrees * 1e2 */
-	float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
-
-	/* GPS position */
-	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
-				     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 * 1e2f, // from m/s to cm/s
-				     cog_deg * 1e2f, // from deg to deg * 100
-				     gps.satellites_visible);
-
-	/* update SAT info every 10 seconds */
-	if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
-		mavlink_msg_gps_status_send(MAVLINK_COMM_0,
-					    gps.satellites_visible,
-					    gps.satellite_prn,
-					    gps.satellite_used,
-					    gps.satellite_elevation,
-					    gps.satellite_azimuth,
-					    gps.satellite_snr);
-	}
-
-	gps_counter++;
-}
-
-void
-l_vehicle_status(const struct listener *l)
-{
-	/* immediately communicate state changes back to user */
-	orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
-
-	/* enable or disable HIL */
-	if (v_status.hil_state == HIL_STATE_ON)
-		set_hil_on_off(true);
-	else if (v_status.hil_state == HIL_STATE_OFF)
-		set_hil_on_off(false);
-
-	/* translate the current syste state to mavlink state and mode */
-	uint8_t mavlink_state = 0;
-	uint8_t mavlink_base_mode = 0;
-	uint32_t mavlink_custom_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-	/* send heartbeat */
-	mavlink_msg_heartbeat_send(chan,
-				   mavlink_system.type,
-				   MAV_AUTOPILOT_PX4,
-				   mavlink_base_mode,
-				   mavlink_custom_mode,
-				   mavlink_state);
-}
-
-void
-l_rc_channels(const struct listener *l)
-{
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
-	// XXX Add RC channels scaled message here
-}
-
-void
-l_input_rc(const struct listener *l)
-{
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
-
-	if (gcs_link) {
-
-		const unsigned port_width = 8;
-
-		for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
-			/* Channels are sent in MAVLink main loop at a fixed interval */
-			mavlink_msg_rc_channels_raw_send(chan,
-							 rc_raw.timestamp_publication / 1000,
-							 i,
-							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
-							 rc_raw.rssi);
-		}
-	}
-}
-
-void
-l_global_position(const struct listener *l)
-{
-	/* copy global position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
-
-	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
-						 global_pos.timestamp / 1000,
-					     global_pos.lat,
-					     global_pos.lon,
-					     global_pos.alt * 1000.0f,
-					     global_pos.relative_alt * 1000.0f,
-					     global_pos.vx * 100.0f,
-					     global_pos.vy * 100.0f,
-					     global_pos.vz * 100.0f,
-					     _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
-}
-
-void
-l_local_position(const struct listener *l)
-{
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
-
-	if (gcs_link)
-		mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
-						    local_pos.timestamp / 1000,
-						    local_pos.x,
-						    local_pos.y,
-						    local_pos.z,
-						    local_pos.vx,
-						    local_pos.vy,
-						    local_pos.vz);
-}
-
-void
-l_global_position_setpoint(const struct listener *l)
-{
-	struct vehicle_global_position_setpoint_s global_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
-
-	uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
-
-	if (global_sp.altitude_is_relative)
-		coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
-
-	if (gcs_link)
-		mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
-				coordinate_frame,
-				global_sp.lat,
-				global_sp.lon,
-				global_sp.altitude * 1000.0f,
-				global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
-}
-
-void
-l_local_position_setpoint(const struct listener *l)
-{
-	struct vehicle_local_position_setpoint_s local_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp);
-
-	if (gcs_link)
-		mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
-				MAV_FRAME_LOCAL_NED,
-				local_sp.x,
-				local_sp.y,
-				local_sp.z,
-				local_sp.yaw);
-}
-
-void
-l_attitude_setpoint(const struct listener *l)
-{
-	struct vehicle_attitude_setpoint_s att_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp);
-
-	if (gcs_link)
-		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
-				att_sp.timestamp / 1000,
-				att_sp.roll_body,
-				att_sp.pitch_body,
-				att_sp.yaw_body,
-				att_sp.thrust);
-}
-
-void
-l_vehicle_rates_setpoint(const struct listener *l)
-{
-	struct vehicle_rates_setpoint_s rates_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
-
-	if (gcs_link)
-		mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0,
-				rates_sp.timestamp / 1000,
-				rates_sp.roll,
-				rates_sp.pitch,
-				rates_sp.yaw,
-				rates_sp.thrust);
-}
-
-void
-l_actuator_outputs(const struct listener *l)
-{
-	struct actuator_outputs_s act_outputs;
-
-	orb_id_t ids[] = {
-		ORB_ID(actuator_outputs_0),
-		ORB_ID(actuator_outputs_1),
-		ORB_ID(actuator_outputs_2),
-		ORB_ID(actuator_outputs_3)
-	};
-
-	/* copy actuator data into local buffer */
-	orb_copy(ids[l->arg], *l->subp, &act_outputs);
-
-	if (gcs_link) {
-		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
-						  l->arg /* port number - needs GCS support */,
-							 /* QGC has port number support already */
-						  act_outputs.output[0],
-						  act_outputs.output[1],
-						  act_outputs.output[2],
-						  act_outputs.output[3],
-						  act_outputs.output[4],
-						  act_outputs.output[5],
-						  act_outputs.output[6],
-						  act_outputs.output[7]);
-
-		/* only send in HIL mode and only send first group for HIL */
-		if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
-
-			/* translate the current syste state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_base_mode = 0;
-			uint32_t mavlink_custom_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-			/* HIL message as per MAVLink spec */
-
-			/* scale / assign outputs depending on system type */
-
-			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      -1,
-							      -1,
-							      -1,
-							      -1,
-							      mavlink_base_mode,
-							      0);
-
-			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-							      -1,
-							      -1,
-							      mavlink_base_mode,
-							      0);
-
-			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-							      mavlink_base_mode,
-							      0);
-
-			} else {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      (act_outputs.output[0] - 1500.0f) / 500.0f,
-							      (act_outputs.output[1] - 1500.0f) / 500.0f,
-							      (act_outputs.output[2] - 1500.0f) / 500.0f,
-							      (act_outputs.output[3] - 1000.0f) / 1000.0f,
-							      (act_outputs.output[4] - 1500.0f) / 500.0f,
-							      (act_outputs.output[5] - 1500.0f) / 500.0f,
-							      (act_outputs.output[6] - 1500.0f) / 500.0f,
-							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-							      mavlink_base_mode,
-							      0);
-			}
-		}
-	}
-}
-
-void
-l_actuator_armed(const struct listener *l)
-{
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
-}
-
-void
-l_manual_control_setpoint(const struct listener *l)
-{
-	struct manual_control_setpoint_s man_control;
-
-	/* copy manual control data into local buffer */
-	orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control);
-
-	if (gcs_link)
-		mavlink_msg_manual_control_send(MAVLINK_COMM_0,
-						mavlink_system.sysid,
-						man_control.roll * 1000,
-						man_control.pitch * 1000,
-						man_control.yaw * 1000,
-						man_control.throttle * 1000,
-						0);
-}
-
-void
-l_vehicle_attitude_controls(const struct listener *l)
-{
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
-
-	if (gcs_link) {
-		/* send, add spaces so that string buffer is at least 10 chars long */
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "ctrl0    ",
-						   actuators_0.control[0]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "ctrl1    ",
-						   actuators_0.control[1]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "ctrl2     ",
-						   actuators_0.control[2]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "ctrl3     ",
-						   actuators_0.control[3]);
-	}
-}
-
-void
-l_debug_key_value(const struct listener *l)
-{
-	struct debug_key_value_s debug;
-
-	orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug);
-
-	/* Enforce null termination */
-	debug.key[sizeof(debug.key) - 1] = '\0';
-
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-					   last_sensor_timestamp / 1000,
-					   debug.key,
-					   debug.value);
-}
-
-void
-l_optical_flow(const struct listener *l)
-{
-	struct optical_flow_s flow;
-
-	orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
-
-	mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
-				      flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
-}
-
-void
-l_home(const struct listener *l)
-{
-	struct home_position_s home;
-
-	orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
-
-	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
-}
-
-void
-l_airspeed(const struct listener *l)
-{
-	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
-}
-
-void
-l_nav_cap(const struct listener *l)
-{
-
-	orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap);
-
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-				   hrt_absolute_time() / 1000,
-				   "turn dist",
-				   nav_cap.turn_distance);
-
-}
-
-static void *
-uorb_receive_thread(void *arg)
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
-
-	/*
-	 * set up poll to block for new data,
-	 * wait for a maximum of 1000 ms (1 second)
-	 */
-	const int timeout = 1000;
-
-	/*
-	 * Initialise listener array.
-	 *
-	 * Might want to invoke each listener once to set initial state.
-	 */
-	struct pollfd fds[n_listeners];
-
-	for (unsigned i = 0; i < n_listeners; i++) {
-		fds[i].fd = *listeners[i].subp;
-		fds[i].events = POLLIN;
-
-		/* Invoke callback to set initial state */
-		//listeners[i].callback(&listener[i]);
-	}
-
-	while (!thread_should_exit) {
-
-		int poll_ret = poll(fds, n_listeners, timeout);
-
-		/* handle the poll result */
-		if (poll_ret == 0) {
-			/* silent */
-
-		} else if (poll_ret < 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
-
-		} else {
-
-			for (unsigned i = 0; i < n_listeners; i++) {
-				if (fds[i].revents & POLLIN)
-					listeners[i].callback(&listeners[i]);
-			}
-		}
-	}
-
-	return NULL;
-}
-
-pthread_t
-uorb_receive_start(void)
-{
-	/* --- SENSORS RAW VALUE --- */
-	mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.sensor_sub, 200);	/* 5Hz updates */
-
-	/* --- ATTITUDE VALUE --- */
-	mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.att_sub, 200);	/* 5Hz updates */
-
-	/* --- GPS VALUE --- */
-	mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	orb_set_interval(mavlink_subs.gps_sub, 200);	/* 5Hz updates */
-
-	/* --- HOME POSITION --- */
-	mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
-	orb_set_interval(mavlink_subs.home_sub, 1000);	/* 1Hz updates */
-
-	/* --- SYSTEM STATE --- */
-	status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */
-
-	/* --- RC CHANNELS VALUE --- */
-	rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	orb_set_interval(rc_sub, 100);			/* 10Hz updates */
-
-	/* --- RC RAW VALUE --- */
-	mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	orb_set_interval(mavlink_subs.input_rc_sub, 100);
-
-	/* --- GLOBAL POS VALUE --- */
-	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(mavlink_subs.global_pos_sub, 100);	/* 10 Hz active updates */
-
-	/* --- LOCAL POS VALUE --- */
-	mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */
-
-	/* --- GLOBAL SETPOINT VALUE --- */
-	mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	orb_set_interval(mavlink_subs.spg_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- LOCAL SETPOINT VALUE --- */
-	mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- ATTITUDE SETPOINT VALUE --- */
-	mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	orb_set_interval(mavlink_subs.spa_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- RATES SETPOINT VALUE --- */
-	mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000);  /* 0.5 Hz updates */
-
-	/* --- ACTUATOR OUTPUTS --- */
-	mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
-	mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
-	mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
-	mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
-	/* rate limits set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.act_0_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_1_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_2_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_3_sub, 100);	/* 10Hz updates */
-
-	/* --- ACTUATOR ARMED VALUE --- */
-	mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(mavlink_subs.armed_sub, 100);	/* 10Hz updates */
-
-	/* --- MAPPED MANUAL CONTROL INPUTS --- */
-	mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	/* rate limits set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
-
-	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
-
-	/* --- DEBUG VALUE OUTPUT --- */
-	mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
-	orb_set_interval(mavlink_subs.debug_key_value, 100);	/* 10Hz updates */
-
-	/* --- FLOW SENSOR --- */
-	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
-	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
-
-	/* --- AIRSPEED --- */
-	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
-
-	/* --- NAVIGATION CAPABILITIES --- */
-	mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
-	orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); 	/* 2Hz updates */
-	nav_cap.turn_distance = 0.0f;
-
-	/* start the listener loop */
-	pthread_attr_t uorb_attr;
-	pthread_attr_init(&uorb_attr);
-
-	/* Set stack size, needs less than 2k */
-	pthread_attr_setstacksize(&uorb_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
-
-	pthread_attr_destroy(&uorb_attr);
-	return thread;
-}
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
deleted file mode 100644
index 7e4a2688f..000000000
--- a/src/modules/mavlink/waypoints.c
+++ /dev/null
@@ -1,1042 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @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 <joes@student.ethz.ch>
- *
- * 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 waypoints.c
- * MAVLink waypoint protocol implementation (BSD-relicensed).
- */
-
-#include <math.h>
-#include <sys/prctl.h>
-#include <unistd.h>
-#include <stdio.h>
-
-#include "mavlink_bridge_header.h"
-#include "missionlib.h"
-#include "waypoints.h"
-#include "util.h"
-
-#ifndef FM_PI
-#define FM_PI 3.1415926535897932384626433832795f
-#endif
-
-bool debug = false;
-bool verbose = false;
-
-
-#define MAVLINK_WPM_NO_PRINTF
-
-uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
-
-void mavlink_wpm_init(mavlink_wpm_storage *state)
-{
-	// Set all waypoints to zero
-
-	// Set count to zero
-	state->size = 0;
-	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
-	state->current_state = MAVLINK_WPM_STATE_IDLE;
-	state->current_partner_sysid = 0;
-	state->current_partner_compid = 0;
-	state->timestamp_lastaction = 0;
-	state->timestamp_last_send_setpoint = 0;
-	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
-	state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
-	state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
-	state->current_active_wp_id = -1;		///< id of current waypoint
-	state->yaw_reached = false;						///< boolean for yaw attitude reached
-	state->pos_reached = false;						///< boolean for position reached
-	state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
-	state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
-
-}
-
-/*
- *  @brief Sends an waypoint ack message
- */
-void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
-{
-	mavlink_message_t msg;
-	mavlink_mission_ack_t wpa;
-
-	wpa.target_system = wpm->current_partner_sysid;
-	wpa.target_component = wpm->current_partner_compid;
-	wpa.type = type;
-
-	mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
-	mavlink_missionlib_send_message(&msg);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-	if (MAVLINK_WPM_TEXT_FEEDBACK) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-#else
-
-		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
-
-#endif
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-	}
-}
-
-/*
- *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
- *
- *  This function broadcasts its new active waypoint sequence number and
- *  sends a message to the controller, advising it to fly to the coordinates
- *  of the waypoint with a given orientation
- *
- *  @param seq The waypoint sequence number the MAV should fly to.
- */
-void mavlink_wpm_send_waypoint_current(uint16_t seq)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
-		mavlink_message_t msg;
-		mavlink_mission_current_t wpc;
-
-		wpc.seq = cur->seq;
-
-		mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
-		mavlink_missionlib_send_message(&msg);
-
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq);
-
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n");
-	}
-}
-
-/*
- *  @brief Directs the MAV to fly to a position
- *
- *  Sends a message to the controller, advising it to fly to the coordinates
- *  of the waypoint with a given orientation
- *
- *  @param seq The waypoint sequence number the MAV should fly to.
- */
-void mavlink_wpm_send_setpoint(uint16_t seq)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-		mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1,
-				cur->param2, cur->param3, cur->param4, cur->x,
-				cur->y, cur->z, cur->frame, cur->command);
-
-		wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
-
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n");
-	}
-}
-
-void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
-{
-	mavlink_message_t msg;
-	mavlink_mission_count_t wpc;
-
-	wpc.target_system = wpm->current_partner_sysid;
-	wpc.target_component = wpm->current_partner_compid;
-	wpc.count = count;
-
-	mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
-	mavlink_missionlib_send_message(&msg);
-
-	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-}
-
-void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
-{
-	if (seq < wpm->size) {
-		mavlink_message_t msg;
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-		wp->target_system = wpm->current_partner_sysid;
-		wp->target_component = wpm->current_partner_compid;
-		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp);
-		mavlink_missionlib_send_message(&msg);
-
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
-
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n");
-	}
-}
-
-void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
-{
-	if (seq < wpm->max_size) {
-		mavlink_message_t msg;
-		mavlink_mission_request_t wpr;
-		wpr.target_system = wpm->current_partner_sysid;
-		wpr.target_component = wpm->current_partner_compid;
-		wpr.seq = seq;
-		mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
-		mavlink_missionlib_send_message(&msg);
-
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
-
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
-	}
-}
-
-/*
- *  @brief emits a message that a waypoint reached
- *
- *  This function broadcasts a message that a waypoint is reached.
- *
- *  @param seq The waypoint sequence number the MAV has reached.
- */
-void mavlink_wpm_send_waypoint_reached(uint16_t seq)
-{
-	mavlink_message_t msg;
-	mavlink_mission_item_reached_t wp_reached;
-
-	wp_reached.seq = seq;
-
-	mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
-	mavlink_missionlib_send_message(&msg);
-
-	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-}
-
-/*
- * Calculate distance in global frame.
- *
- * The distance calculation is based on the WGS84 geoid (GPS)
- */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
-{
-
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-
-		double current_x_rad = wp->x / 180.0 * M_PI;
-		double current_y_rad = wp->y / 180.0 * M_PI;
-		double x_rad = lat / 180.0 * M_PI;
-		double y_rad = lon / 180.0 * M_PI;
-
-		double d_lat = x_rad - current_x_rad;
-		double d_lon = y_rad - current_y_rad;
-
-		double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
-		double c = 2 * atan2(sqrt(a), sqrt(1 - a));
-
-		const double radius_earth = 6371000.0;
-
-		float dxy = radius_earth * c;
-		float dz = alt - wp->z;
-
-		*dist_xy = fabsf(dxy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dxy * dxy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-
-}
-
-/*
- * Calculate distance in local frame (NED)
- */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
-		float dx = (cur->x - x);
-		float dy = (cur->y - y);
-		float dz = (cur->z - z);
-
-		*dist_xy = sqrtf(dx * dx + dy * dy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dx * dx + dy * dy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-}
-
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
-{
-	static uint16_t counter;
-
-	if ((!global_pos->valid && !local_pos->xy_valid) ||
-		/* no waypoint */
-		wpm->size == 0) {
-		/* nothing to check here, return */
-		return;
-	}
-
-	if (wpm->current_active_wp_id < wpm->size) {
-
-		float orbit;
-		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
-
-		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
-		} else {
-
-			// XXX set default orbit via param
-			orbit = 15.0f;
-		}
-
-		/* keep vertical orbit */
-		float vertical_switch_distance = orbit;
-
-		/* Take the larger turn distance - orbit or turn_distance */
-		if (orbit < turn_distance)
-			orbit = turn_distance;
-
-		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
-		float dist = -1.0f;
-
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
-			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
-			/* Check if conditions of mission item are satisfied */
-			// XXX TODO
-		}
-
-		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
-			wpm->pos_reached = true;
-		}
-
-		// check if required yaw reached
-		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
-		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
-		if (fabsf(yaw_err) < 0.05f) {
-			wpm->yaw_reached = true;
-		}
-	}
-
-	//check if the current waypoint was reached
-	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
-		if (wpm->current_active_wp_id < wpm->size) {
-			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-
-			if (wpm->timestamp_firstinside_orbit == 0) {
-				// Announce that last waypoint was reached
-				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
-				wpm->timestamp_firstinside_orbit = now;
-			}
-
-			// check if the MAV was long enough inside the waypoint orbit
-			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
-
-			bool time_elapsed = false;
-
-			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-				time_elapsed = true;
-			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
-				time_elapsed = true;
-			}
-
-			if (time_elapsed) {
-
-				/* safeguard against invalid missions with last wp autocontinue on */
-				if (wpm->current_active_wp_id == wpm->size - 1) {
-					/* stop handling missions here */
-					cur_wp->autocontinue = false;
-				}
-
-				if (cur_wp->autocontinue) {
-
-					cur_wp->current = 0;
-
-					float navigation_lat = -1.0f;
-					float navigation_lon = -1.0f;
-					float navigation_alt = -1.0f;
-					int navigation_frame = -1;
-
-					/* initialize to current position in case we don't find a suitable navigation waypoint */
-					if (global_pos->valid) {
-						navigation_lat = global_pos->lat/1e7;
-						navigation_lon = global_pos->lon/1e7;
-						navigation_alt = global_pos->alt;
-						navigation_frame = MAV_FRAME_GLOBAL;
-					} else if (local_pos->xy_valid && local_pos->z_valid) {
-						navigation_lat = local_pos->x;
-						navigation_lon = local_pos->y;
-						navigation_alt = local_pos->z;
-						navigation_frame = MAV_FRAME_LOCAL_NED;
-					}
-
-					/* guard against missions without final land waypoint */
-					/* only accept supported navigation waypoints, skip unknown ones */
-					do {
-
-						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
-						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
-
-							/* this is a navigation waypoint */
-							navigation_frame = cur_wp->frame;
-							navigation_lat = cur_wp->x;
-							navigation_lon = cur_wp->y;
-							navigation_alt = cur_wp->z;
-						}
-
-						if (wpm->current_active_wp_id == wpm->size - 1) {
-
-							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
-							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
-								/* the last waypoint was reached, if auto continue is
-								 * activated AND it is NOT a land waypoint, keep the system loitering there.
-								 */
-								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-								cur_wp->frame = navigation_frame;
-								cur_wp->x = navigation_lat;
-								cur_wp->y = navigation_lon;
-								cur_wp->z = navigation_alt;
-							}
-
-							/* we risk an endless loop for missions without navigation waypoints, abort. */
-							break;
-
-						} else {
-							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-								wpm->current_active_wp_id++;
-						}
-			
-					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
-
-					// Fly to next waypoint
-					wpm->timestamp_firstinside_orbit = 0;
-					mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-					mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-					wpm->waypoints[wpm->current_active_wp_id].current = true;
-					wpm->pos_reached = false;
-					wpm->yaw_reached = false;
-					printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
-				}
-			}
-		}
-
-	} else {
-		wpm->timestamp_lastoutside_orbit = now;
-	}
-
-	counter++;
-}
-
-
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
-{
-	/* check for timed-out operations */
-	if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE");
-#else
-
-		if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state);
-
-#endif
-		wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-		wpm->current_count = 0;
-		wpm->current_partner_sysid = 0;
-		wpm->current_partner_compid = 0;
-		wpm->current_wp_id = -1;
-
-		if (wpm->size == 0) {
-			wpm->current_active_wp_id = -1;
-		}
-	}
-
-	check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
-
-	return OK;
-}
-
-
-void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos)
-{
-	uint64_t now = mavlink_missionlib_get_system_timestamp();
-
-	switch (msg->msgid) {
-
-	case MAVLINK_MSG_ID_MISSION_ACK: {
-			mavlink_mission_ack_t wpa;
-			mavlink_msg_mission_ack_decode(msg, &wpa);
-
-			if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-					if (wpm->current_wp_id == wpm->size - 1) {
-
-						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-
-						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-						wpm->current_wp_id = 0;
-					}
-				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
-			mavlink_mission_set_current_t wpc;
-			mavlink_msg_mission_set_current_decode(msg, &wpc);
-
-			if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-					if (wpc.seq < wpm->size) {
-						// if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
-						wpm->current_active_wp_id = wpc.seq;
-						uint32_t i;
-
-						for (i = 0; i < wpm->size; i++) {
-							if (i == wpm->current_active_wp_id) {
-								wpm->waypoints[i].current = true;
-
-							} else {
-								wpm->waypoints[i].current = false;
-							}
-						}
-
-						mavlink_missionlib_send_gcs_string("NEW WP SET");
-
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-						mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-						wpm->timestamp_firstinside_orbit = 0;
-
-					} else {
-						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-					}
-
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-
-				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
-			mavlink_mission_request_list_t wprl;
-			mavlink_msg_mission_request_list_decode(msg, &wprl);
-
-			if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-					if (wpm->size > 0) {
-						//if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
-//                        if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
-						wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
-						wpm->current_wp_id = 0;
-						wpm->current_partner_sysid = msg->sysid;
-						wpm->current_partner_compid = msg->compid;
-
-					} else {
-						// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
-					}
-
-					wpm->current_count = wpm->size;
-					mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count);
-
-				} else {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state);
-				}
-			} else {
-				// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_REQUEST: {
-			mavlink_mission_request_t wpr;
-			mavlink_msg_mission_request_decode(msg, &wpr);
-
-			if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
-				if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) {
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
-
-#endif
-					}
-
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
-
-#endif
-					}
-
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
-
-#endif
-					}
-
-					wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
-					wpm->current_wp_id = wpr.seq;
-					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq);
-
-				} else {
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state);
-
-#endif
-							break;
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-							if (wpr.seq != 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
-
-#endif
-							}
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-							if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
-
-#endif
-
-							} else if (wpr.seq >= wpm->size) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
-
-#endif
-							}
-
-						} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
-#else
-
-							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n");
-
-#endif
-						}
-					}
-				}
-
-			} else {
-				//we we're target but already communicating with someone else
-				if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-					if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid);
-
-#endif
-
-				} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
-
-					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
-				}
-
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_COUNT: {
-			mavlink_mission_count_t wpc;
-			mavlink_msg_mission_count_decode(msg, &wpc);
-
-			if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) {
-//                	printf("wpc count in: %d\n",wpc.count);
-//                	printf("Comp id: %d\n",msg->compid);
-//                	printf("Current partner sysid: %d\n",wpm->current_partner_sysid);
-
-					if (wpc.count > 0) {
-						if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST");
-#else
-
-							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
-
-#endif
-						}
-
-						if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
-#else
-
-							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
-
-#endif
-						}
-
-						wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
-						wpm->current_wp_id = 0;
-						wpm->current_partner_sysid = msg->sysid;
-						wpm->current_partner_compid = msg->compid;
-						wpm->current_count = wpc.count;
-
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
-
-#endif
-						wpm->rcv_size = 0;
-						//while(waypoints_receive_buffer->size() > 0)
-//                        {
-//                            delete waypoints_receive_buffer->back();
-//                            waypoints_receive_buffer->pop_back();
-//                        }
-
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-
-					} else if (wpc.count == 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("COUNT 0");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
-
-#endif
-						wpm->rcv_size = 0;
-						//while(waypoints_receive_buffer->size() > 0)
-//                        {
-//                            delete waypoints->back();
-//                            waypoints->pop_back();
-//                        }
-						wpm->current_active_wp_id = -1;
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						break;
-
-					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("IGN WP CMD");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
-
-#endif
-					}
-
-				} else {
-					if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state);
-
-#endif
-
-					} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id);
-
-#endif
-
-					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n");
-
-#endif
-					}
-				}
-
-			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
-
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
-			}
-
-		}
-		break;
-
-	case MAVLINK_MSG_ID_MISSION_ITEM: {
-			mavlink_mission_item_t wp;
-			mavlink_msg_mission_item_decode(msg, &wp);
-
-			mavlink_missionlib_send_gcs_string("GOT WP");
-//			printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid);
-//			printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid);
-
-//            if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/))
-			if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
-
-				wpm->timestamp_lastaction = now;
-
-//                printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id);
-
-//                wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO
-
-				//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
-				if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) {
-					//mavlink_missionlib_send_gcs_string("DEBUG 2");
-
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid);
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
-//
-					wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
-					mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
-					memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
-//                    printf("WP seq: %d\n",wp.seq);
-					wpm->current_wp_id = wp.seq + 1;
-
-					// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
-//					printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
-
-//					printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count);
-					if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-						mavlink_missionlib_send_gcs_string("GOT ALL WPS");
-						// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count);
-
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
-
-						if (wpm->current_active_wp_id > wpm->rcv_size - 1) {
-							wpm->current_active_wp_id = wpm->rcv_size - 1;
-						}
-
-						// switch the waypoints list
-						// FIXME CHECK!!!
-						uint32_t i;
-
-						for (i = 0; i < wpm->current_count; ++i) {
-							wpm->waypoints[i] = wpm->rcv_waypoints[i];
-						}
-
-						wpm->size = wpm->current_count;
-
-						//get the new current waypoint
-
-						for (i = 0; i < wpm->size; i++) {
-							if (wpm->waypoints[i].current == 1) {
-								wpm->current_active_wp_id = i;
-								//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
-								wpm->yaw_reached = false;
-								wpm->pos_reached = false;
-								mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-								mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-								wpm->timestamp_firstinside_orbit = 0;
-								break;
-							}
-						}
-
-						if (i == wpm->size) {
-							wpm->current_active_wp_id = -1;
-							wpm->yaw_reached = false;
-							wpm->pos_reached = false;
-							wpm->timestamp_firstinside_orbit = 0;
-						}
-
-						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-
-					} else {
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-					}
-
-				} else {
-					if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-						//we're done receiving waypoints, answer with ack.
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
-						printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
-					}
-
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
-							break;
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-							if (!(wp.seq == 0)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-							if (!(wp.seq == wpm->current_wp_id)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
-								mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-
-							} else if (!(wp.seq < wpm->current_count)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-						}
-					}
-				}
-			} else {
-				//we we're target but already communicating with someone else
-				if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid);
-				} else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
-				}
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
-			mavlink_mission_clear_all_t wpca;
-			mavlink_msg_mission_clear_all_decode(msg, &wpca);
-
-			if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-				wpm->timestamp_lastaction = now;
-
-				// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
-				// Delete all waypoints
-				wpm->size = 0;
-				wpm->current_active_wp_id = -1;
-				wpm->yaw_reached = false;
-				wpm->pos_reached = false;
-
-			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-				// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state);
-			}
-
-			break;
-		}
-
-	default: {
-			// if (debug) // printf("Waypoint: received message of unknown type");
-			break;
-		}
-	}
-
-	// check_waypoints_reached(now, global_pos, local_pos);
-}
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
deleted file mode 100644
index d7d6b31dc..000000000
--- a/src/modules/mavlink/waypoints.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author 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
- * 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 waypoints.h
- * MAVLink waypoint protocol definition (BSD-relicensed).
- */
-
-#ifndef WAYPOINTS_H_
-#define WAYPOINTS_H_
-
-/* This assumes you have the mavlink headers on your include path
- or in the same folder as this source file */
-
-#include <v1.0/mavlink_types.h>
-
-// #ifndef MAVLINK_SEND_UART_BYTES
-// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
-// #endif
-//extern mavlink_system_t mavlink_system;
-#include "mavlink_bridge_header.h"
-#include <stdbool.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/navigation_capabilities.h>
-
-// FIXME XXX - TO BE MOVED TO XML
-enum MAVLINK_WPM_STATES {
-	MAVLINK_WPM_STATE_IDLE = 0,
-	MAVLINK_WPM_STATE_SENDLIST,
-	MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
-	MAVLINK_WPM_STATE_GETLIST,
-	MAVLINK_WPM_STATE_GETLIST_GETWPS,
-	MAVLINK_WPM_STATE_GETLIST_GOTALL,
-	MAVLINK_WPM_STATE_ENUM_END
-};
-
-enum MAVLINK_WPM_CODES {
-	MAVLINK_WPM_CODE_OK = 0,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
-	MAVLINK_WPM_CODE_ENUM_END
-};
-
-
-/* WAYPOINT MANAGER - MISSION LIB */
-
-#define MAVLINK_WPM_MAX_WP_COUNT 15
-#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE				  ///< Enable double buffer and in-flight updates
-#ifndef MAVLINK_WPM_TEXT_FEEDBACK
-#define MAVLINK_WPM_TEXT_FEEDBACK 0						  ///< Report back status information as text
-#endif
-#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000         ///< Protocol communication timeout in useconds
-#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000           ///< When to send a new setpoint
-#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
-
-
-struct mavlink_wpm_storage {
-	mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
-#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
-	mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
-#endif
-	uint16_t size;
-	uint16_t max_size;
-	uint16_t rcv_size;
-	enum MAVLINK_WPM_STATES current_state;
-	int16_t current_wp_id;							///< Waypoint in current transmission
-	int16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
-	uint16_t current_count;
-	uint8_t current_partner_sysid;
-	uint8_t current_partner_compid;
-	uint64_t timestamp_lastaction;
-	uint64_t timestamp_last_send_setpoint;
-	uint64_t timestamp_firstinside_orbit;
-	uint64_t timestamp_lastoutside_orbit;
-	uint32_t timeout;
-	uint32_t delay_setpoint;
-	float accept_range_yaw;
-	float accept_range_distance;
-	bool yaw_reached;
-	bool pos_reached;
-	bool idle;
-};
-
-typedef struct mavlink_wpm_storage mavlink_wpm_storage;
-
-void mavlink_wpm_init(mavlink_wpm_storage *state);
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
-			       struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
-void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
-				 struct vehicle_local_position_s *local_pos);
-
-extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
-
-#endif /* WAYPOINTS_H_ */
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
deleted file mode 100644
index 0edb53a3e..000000000
--- a/src/modules/mavlink_onboard/mavlink.c
+++ /dev/null
@@ -1,536 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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
- * 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.c
- * MAVLink 1.0 protocol implementation.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-
-#include "orb_topics.h"
-#include "util.h"
-
-__EXPORT int mavlink_onboard_main(int argc, char *argv[]);
-
-static int mavlink_thread_main(int argc, char *argv[]);
-
-/* thread state */
-volatile bool thread_should_exit = false;
-static volatile bool thread_running = false;
-static int mavlink_task;
-
-/* pthreads */
-static pthread_t receive_thread;
-
-/* terminate MAVLink on user request - disabled by default */
-static bool mavlink_link_termination_allowed = false;
-
-mavlink_system_t mavlink_system = {
-	100,
-	50,
-	MAV_TYPE_QUADROTOR,
-	0,
-	0,
-	0
-}; // System ID, 1-255, Component/Subsystem ID, 1-255
-
-/* XXX not widely used */
-uint8_t chan = MAVLINK_COMM_0;
-
-/* XXX probably should be in a header... */
-extern pthread_t receive_start(int uart);
-
-bool mavlink_hil_enabled = false;
-
-/* protocol interface */
-static int uart;
-static int baudrate;
-bool gcs_link = true;
-
-/* interface mode */
-static enum {
-	MAVLINK_INTERFACE_MODE_OFFBOARD,
-	MAVLINK_INTERFACE_MODE_ONBOARD
-} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
-
-static void mavlink_update_system(void);
-static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-static void usage(void);
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
-{
-	/* process baud rate */
-	int speed;
-
-	switch (baud) {
-	case 0:      speed = B0;      break;
-
-	case 50:     speed = B50;     break;
-
-	case 75:     speed = B75;     break;
-
-	case 110:    speed = B110;    break;
-
-	case 134:    speed = B134;    break;
-
-	case 150:    speed = B150;    break;
-
-	case 200:    speed = B200;    break;
-
-	case 300:    speed = B300;    break;
-
-	case 600:    speed = B600;    break;
-
-	case 1200:   speed = B1200;   break;
-
-	case 1800:   speed = B1800;   break;
-
-	case 2400:   speed = B2400;   break;
-
-	case 4800:   speed = B4800;   break;
-
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-	case 460800: speed = B460800; break;
-
-	case 921600: speed = B921600; break;
-
-	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud);
-		return -EINVAL;
-	}
-
-	/* open uart */
-	warnx("UART is %s, baudrate is %d", uart_name, baud);
-	uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-	*is_usb = false;
-
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK) {
-		/* Back up the original uart configuration to restore it after exit */
-		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-			warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
-
-		/* Fill the struct for the new configuration */
-		tcgetattr(uart, &uart_config);
-
-		/* Clear ONLCR flag (which appends a CR for every LF) */
-		uart_config.c_oflag &= ~ONLCR;
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
-
-
-		if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name);
-			close(uart);
-			return -1;
-		}
-
-	} else {
-		*is_usb = true;
-	}
-
-	return uart;
-}
-
-void 
-mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
-{
-	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
-}
-
-/*
- * Internal function to give access to the channel status for each channel
- */
-mavlink_status_t* mavlink_get_channel_status(uint8_t channel)
-{
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[channel];
-}
-
-/*
- * Internal function to give access to the channel buffer for each channel
- */
-mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel)
-{
-	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_buffer[channel];
-}
-
-void mavlink_update_system(void)
-{
-	static bool initialized = false;
-	param_t param_system_id;
-	param_t param_component_id;
-	param_t param_system_type;
-
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-	}
-
-	/* update system and component id */
-	int32_t system_id;
-	param_get(param_system_id, &system_id);
-	if (system_id > 0 && system_id < 255) {
-		mavlink_system.sysid = system_id;
-	}
-
-	int32_t component_id;
-	param_get(param_component_id, &component_id);
-	if (component_id > 0 && component_id < 255) {
-		mavlink_system.compid = component_id;
-	}
-	
-	int32_t system_type;
-	param_get(param_system_type, &system_type);
-	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
-	}
-}
-
-void
-get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
-	uint8_t *mavlink_state, uint8_t *mavlink_mode)
-{
-	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
-
-	/* set mode flags independent of system state */
-	if (control_mode->flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-	}
-
-	if (control_mode->flag_system_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed->armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	if (control_mode->flag_control_velocity_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
-	}
-
-//	switch (v_status->state_machine) {
-//	case SYSTEM_STATE_PREFLIGHT:
-//		if (v_status->flag_preflight_gyro_calibration ||
-//		    v_status->flag_preflight_mag_calibration ||
-//		    v_status->flag_preflight_accel_calibration) {
-//			*mavlink_state = MAV_STATE_CALIBRATING;
-//		} else {
-//			*mavlink_state = MAV_STATE_UNINIT;
-//		}
-//		break;
-//
-//	case SYSTEM_STATE_STANDBY:
-//		*mavlink_state = MAV_STATE_STANDBY;
-//		break;
-//
-//	case SYSTEM_STATE_GROUND_READY:
-//		*mavlink_state = MAV_STATE_ACTIVE;
-//		break;
-//
-//	case SYSTEM_STATE_MANUAL:
-//		*mavlink_state = MAV_STATE_ACTIVE;
-//		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-//		break;
-//
-//	case SYSTEM_STATE_STABILIZED:
-//		*mavlink_state = MAV_STATE_ACTIVE;
-//		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-//		break;
-//
-//	case SYSTEM_STATE_AUTO:
-//		*mavlink_state = MAV_STATE_ACTIVE;
-//		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-//		break;
-//
-//	case SYSTEM_STATE_MISSION_ABORT:
-//		*mavlink_state = MAV_STATE_EMERGENCY;
-//		break;
-//
-//	case SYSTEM_STATE_EMCY_LANDING:
-//		*mavlink_state = MAV_STATE_EMERGENCY;
-//		break;
-//
-//	case SYSTEM_STATE_EMCY_CUTOFF:
-//		*mavlink_state = MAV_STATE_EMERGENCY;
-//		break;
-//
-//	case SYSTEM_STATE_GROUND_ERROR:
-//		*mavlink_state = MAV_STATE_EMERGENCY;
-//		break;
-//
-//	case SYSTEM_STATE_REBOOT:
-//		*mavlink_state = MAV_STATE_POWEROFF;
-//		break;
-//	}
-
-}
-
-/**
- * MAVLink Protocol main function.
- */
-int mavlink_thread_main(int argc, char *argv[])
-{
-	int ch;
-	char *device_name = "/dev/ttyS1";
-	baudrate = 57600;
-
-	/* XXX this is never written? */
-	struct vehicle_status_s v_status;
-	struct vehicle_control_mode_s control_mode;
-	struct actuator_armed_s armed;
-
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-
-	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
-		switch (ch) {
-		case 'b':
-			baudrate = strtoul(optarg, NULL, 10);
-			if (baudrate == 0)
-				errx(1, "invalid baud rate '%s'", optarg);
-			break;
-
-		case 'd':
-			device_name = optarg;
-			break;
-
-		case 'e':
-			mavlink_link_termination_allowed = true;
-			break;
-
-		case 'o':
-			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
-			break;
-
-		default:
-			usage();
-		}
-	}
-
-	struct termios uart_config_original;
-	bool usb_uart;
-
-	/* print welcome text */
-	warnx("MAVLink v1.0 serial interface starting...");
-
-	/* inform about mode */
-	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
-
-	/* Flush stdout in case MAVLink is about to take it over */
-	fflush(stdout);
-
-	/* default values for arguments */
-	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
-	if (uart < 0)
-		err(1, "could not open %s", device_name);
-
-	/* Initialize system properties */
-	mavlink_update_system();
-
-	/* start the MAVLink receiver */
-	receive_thread = receive_start(uart);
-
-	thread_running = true;
-
-	/* arm counter to go off immediately */
-	unsigned lowspeed_counter = 10;
-
-	while (!thread_should_exit) {
-
-		/* 1 Hz */
-		if (lowspeed_counter == 10) {
-			mavlink_update_system();
-
-			/* translate the current system state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
-
-			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
-
-			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(chan,
-						    v_status.onboard_control_sensors_present,
-						    v_status.onboard_control_sensors_enabled,
-						    v_status.onboard_control_sensors_health,
-						    v_status.load * 1000.0f,
-						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 1000.0f,
-						    v_status.battery_remaining,
-						    v_status.drop_rate_comm,
-						    v_status.errors_comm,
-						    v_status.errors_count1,
-						    v_status.errors_count2,
-						    v_status.errors_count3,
-						    v_status.errors_count4);
-			lowspeed_counter = 0;
-		}
-		lowspeed_counter++;
-
-		/* sleep 1000 ms */
-		usleep(1000000);
-	}
-
-	/* wait for threads to complete */
-	pthread_join(receive_thread, NULL);
-
-	/* Reset the UART flags to original state */
-	if (!usb_uart)
-		tcsetattr(uart, TCSANOW, &uart_config_original);
-
-	thread_running = false;
-
-	exit(0);
-}
-
-static void
-usage()
-{
-	fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n"
-			"       mavlink_onboard stop\n"
-			"       mavlink_onboard status\n");
-	exit(1);
-}
-
-int mavlink_onboard_main(int argc, char *argv[])
-{
-
-	if (argc < 2) {
-		warnx("missing command");
-		usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		/* this is not an error */
-		if (thread_running)
-			errx(0, "already running");
-
-		thread_should_exit = false;
-		mavlink_task = task_spawn_cmd("mavlink_onboard",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  2048,
-					  mavlink_thread_main,
-					  (const char**)argv);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		while (thread_running) {
-			usleep(200000);
-		}
-		warnx("terminated");
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			errx(0, "running");
-		} else {
-			errx(1, "not running");
-		}
-	}
-
-	warnx("unrecognized command");
-	usage();
-	/* not getting here */
-	return 0;
-}
-
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
deleted file mode 100644
index 4658bcc1d..000000000
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ /dev/null
@@ -1,344 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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
- * 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_receiver.c
- * MAVLink protocol message receive and dispatch
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* XXX trim includes */
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-
-#include "util.h"
-#include "orb_topics.h"
-
-/* XXX should be in a header somewhere */
-pthread_t receive_start(int uart);
-
-static void handle_message(mavlink_message_t *msg);
-static void *receive_thread(void *arg);
-
-static mavlink_status_t status;
-static struct vehicle_vicon_position_s vicon_position;
-static struct vehicle_command_s vcmd;
-static struct offboard_control_setpoint_s offboard_control_sp;
-
-struct vehicle_global_position_s hil_global_pos;
-struct vehicle_attitude_s hil_attitude;
-orb_advert_t pub_hil_global_pos = -1;
-orb_advert_t pub_hil_attitude = -1;
-
-static orb_advert_t cmd_pub = -1;
-static orb_advert_t flow_pub = -1;
-
-static orb_advert_t offboard_control_sp_pub = -1;
-static orb_advert_t vicon_position_pub = -1;
-
-extern bool gcs_link;
-
-static void
-handle_message(mavlink_message_t *msg)
-{
-	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
-
-		mavlink_command_long_t cmd_mavlink;
-		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
-
-		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
-				|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
-			//check for MAVLINK terminate command
-			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
-				/* This is the link shutdown command, terminate mavlink */
-				warnx("terminating...");
-				fflush(stdout);
-				usleep(50000);
-
-				/* terminate other threads and this thread */
-				thread_should_exit = true;
-
-			} else {
-
-				/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-				vcmd.param1 = cmd_mavlink.param1;
-				vcmd.param2 = cmd_mavlink.param2;
-				vcmd.param3 = cmd_mavlink.param3;
-				vcmd.param4 = cmd_mavlink.param4;
-				vcmd.param5 = cmd_mavlink.param5;
-				vcmd.param6 = cmd_mavlink.param6;
-				vcmd.param7 = cmd_mavlink.param7;
-				vcmd.command = cmd_mavlink.command;
-				vcmd.target_system = cmd_mavlink.target_system;
-				vcmd.target_component = cmd_mavlink.target_component;
-				vcmd.source_system = msg->sysid;
-				vcmd.source_component = msg->compid;
-				vcmd.confirmation =  cmd_mavlink.confirmation;
-
-				/* check if topic is advertised */
-				if (cmd_pub <= 0) {
-					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
-				}
-
-				/* publish */
-				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-			}
-		}
-	}
-
-	if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
-		mavlink_optical_flow_t flow;
-		mavlink_msg_optical_flow_decode(msg, &flow);
-
-		struct optical_flow_s f;
-
-		f.timestamp = hrt_absolute_time();
-		f.flow_raw_x = flow.flow_x;
-		f.flow_raw_y = flow.flow_y;
-		f.flow_comp_x_m = flow.flow_comp_m_x;
-		f.flow_comp_y_m = flow.flow_comp_m_y;
-		f.ground_distance_m = flow.ground_distance;
-		f.quality = flow.quality;
-		f.sensor_id = flow.sensor_id;
-
-		/* check if topic is advertised */
-		if (flow_pub <= 0) {
-			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
-
-		} else {
-			/* publish */
-			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
-		}
-
-	}
-
-	if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
-		/* Set mode on request */
-		mavlink_set_mode_t new_mode;
-		mavlink_msg_set_mode_decode(msg, &new_mode);
-
-		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = new_mode.custom_mode;
-		vcmd.param3 = 0;
-		vcmd.param4 = 0;
-		vcmd.param5 = 0;
-		vcmd.param6 = 0;
-		vcmd.param7 = 0;
-		vcmd.command = MAV_CMD_DO_SET_MODE;
-		vcmd.target_system = new_mode.target_system;
-		vcmd.target_component = MAV_COMP_ID_ALL;
-		vcmd.source_system = msg->sysid;
-		vcmd.source_component = msg->compid;
-		vcmd.confirmation = 1;
-
-		/* check if topic is advertised */
-		if (cmd_pub <= 0) {
-			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
-
-		} else {
-			/* create command */
-			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-		}
-	}
-
-	/* Handle Vicon position estimates */
-	if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
-		mavlink_vicon_position_estimate_t pos;
-		mavlink_msg_vicon_position_estimate_decode(msg, &pos);
-
-		vicon_position.x = pos.x;
-		vicon_position.y = pos.y;
-		vicon_position.z = pos.z;
-
-		if (vicon_position_pub <= 0) {
-			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
-		}
-	}
-
-	/* Handle quadrotor motor setpoints */
-
-	if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
-		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
-		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
-
-		if (mavlink_system.sysid < 4) {
-
-			/* switch to a receiving link mode */
-			gcs_link = false;
-
-			/*
-			 * rate control mode - defined by MAVLink
-			 */
-
-			uint8_t ml_mode = 0;
-			bool ml_armed = false;
-
-			switch (quad_motors_setpoint.mode) {
-			case 0:
-				ml_armed = false;
-				break;
-
-			case 1:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-				ml_armed = true;
-
-				break;
-
-			case 2:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-				ml_armed = true;
-
-				break;
-
-			case 3:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-				break;
-
-			case 4:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-				break;
-			}
-
-			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
-
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
-				ml_armed = false;
-			}
-
-			offboard_control_sp.armed = ml_armed;
-			offboard_control_sp.mode = ml_mode;
-
-			offboard_control_sp.timestamp = hrt_absolute_time();
-
-			/* check if topic has to be advertised */
-			if (offboard_control_sp_pub <= 0) {
-				offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
-
-			} else {
-				/* Publish */
-				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
-			}
-		}
-	}
-
-}
-
-
-/**
- * Receive data from UART.
- */
-static void *
-receive_thread(void *arg)
-{
-	int uart_fd = *((int *)arg);
-
-	const int timeout = 1000;
-	uint8_t buf[32];
-
-	mavlink_message_t msg;
-
-	prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid());
-
-	struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
-
-	ssize_t nread = 0;
-
-	while (!thread_should_exit) {
-		if (poll(fds, 1, timeout) > 0) {
-			if (nread < sizeof(buf)) {
-				/* to avoid reading very small chunks wait for data before reading */
-				usleep(1000);
-			}
-
-			/* non-blocking read. read may return negative values */
-			nread = read(uart_fd, buf, sizeof(buf));
-
-			/* if read failed, this loop won't execute */
-			for (ssize_t i = 0; i < nread; i++) {
-				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
-					/* handle generic messages and commands */
-					handle_message(&msg);
-				}
-			}
-		}
-	}
-
-	return NULL;
-}
-
-pthread_t
-receive_start(int uart)
-{
-	pthread_attr_t receiveloop_attr;
-	pthread_attr_init(&receiveloop_attr);
-
-	struct sched_param param;
-	param.sched_priority = SCHED_PRIORITY_MAX - 40;
-	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
-
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
-	return thread;
-}
diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk
deleted file mode 100644
index a7a4980fa..000000000
--- a/src/modules/mavlink_onboard/module.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (C) 2012-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.
-#
-############################################################################
-
-#
-# MAVLink protocol to uORB interface process (XXX hack for onboard use)
-#
-
-MODULE_COMMAND	 = mavlink_onboard
-SRCS		 = mavlink.c \
-		   mavlink_receiver.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
deleted file mode 100644
index 1b49c9ce4..000000000
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file orb_topics.h
- * Common sets of topics subscribed to or published by the MAVLink driver,
- * and structures maintained by those subscriptions.
- */
-#pragma once
-
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/rc_channels.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/debug_key_value.h>
-#include <drivers/drv_rc_input.h>
-
-struct mavlink_subscriptions {
-	int sensor_sub;
-	int att_sub;
-	int global_pos_sub;
-	int act_0_sub;
-	int act_1_sub;
-	int act_2_sub;
-	int act_3_sub;
-	int gps_sub;
-	int man_control_sp_sub;
-	int safety_sub;
-	int actuators_sub;
-	int local_pos_sub;
-	int spa_sub;
-	int spl_sub;
-	int spg_sub;
-	int debug_key_value;
-	int input_rc_sub;
-};
-
-extern struct mavlink_subscriptions mavlink_subs;
-
-/** Global position */
-extern struct vehicle_global_position_s global_pos;
-
-/** Local position */
-extern struct vehicle_local_position_s local_pos;
-
-/** Vehicle status */
-// extern struct vehicle_status_s v_status;
-
-/** RC channels */
-extern struct rc_channels_s rc;
-
-/** Actuator armed state */
-// extern struct actuator_armed_s armed;
-
-/** Worker thread starter */
-extern pthread_t uorb_receive_start(void);
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
new file mode 100644
index 000000000..19c10198c
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -0,0 +1,935 @@
+/****************************************************************************
+ *
+ *   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 mc_att_control_main.cpp
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+/**
+ * Multicopter attitude control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControl();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~MulticopterAttitudeControl();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool	_task_should_exit;		/**< if true, sensor task should exit */
+	int		_control_task;			/**< task handle for sensor task */
+
+	int		_v_att_sub;				/**< vehicle attitude subscription */
+	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
+	int		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
+	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
+	int		_params_sub;			/**< parameter updates subscription */
+	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
+	int		_armed_sub;				/**< arming status subscription */
+
+	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
+	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+
+	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
+
+	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
+	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
+	struct manual_control_setpoint_s	_manual_control_sp;	/**< manual control setpoint */
+	struct vehicle_control_mode_s		_v_control_mode;	/**< vehicle control mode */
+	struct actuator_controls_s			_actuators;			/**< actuator controls */
+	struct actuator_armed_s				_armed;				/**< actuator arming status */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
+	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
+	math::Vector<3>		_rates_int;		/**< angular rates integral error */
+	float				_thrust_sp;		/**< thrust setpoint */
+	math::Vector<3>		_att_control;	/**< attitude control vector */
+
+	math::Matrix<3, 3>  _I;				/**< identity matrix */
+
+	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
+
+	struct {
+		param_t roll_p;
+		param_t roll_rate_p;
+		param_t roll_rate_i;
+		param_t roll_rate_d;
+		param_t pitch_p;
+		param_t pitch_rate_p;
+		param_t pitch_rate_i;
+		param_t pitch_rate_d;
+		param_t yaw_p;
+		param_t yaw_rate_p;
+		param_t yaw_rate_i;
+		param_t yaw_rate_d;
+		param_t yaw_ff;
+		param_t yaw_rate_max;
+
+		param_t man_roll_max;
+		param_t man_pitch_max;
+		param_t man_yaw_max;
+		param_t acro_roll_max;
+		param_t acro_pitch_max;
+		param_t acro_yaw_max;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		math::Vector<3> att_p;					/**< P gain for angular error */
+		math::Vector<3> rate_p;				/**< P gain for angular rate error */
+		math::Vector<3> rate_i;				/**< I gain for angular rate error */
+		math::Vector<3> rate_d;				/**< D gain for angular rate error */
+		float yaw_ff;						/**< yaw control feed-forward */
+		float yaw_rate_max;					/**< max yaw rate */
+
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
+		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
+	}		_params;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+
+	/**
+	 * Check for parameter update and handle it.
+	 */
+	void		parameter_update_poll();
+
+	/**
+	 * Check for changes in vehicle control mode.
+	 */
+	void		vehicle_control_mode_poll();
+
+	/**
+	 * Check for changes in manual inputs.
+	 */
+	void		vehicle_manual_poll();
+
+	/**
+	 * Check for attitude setpoint updates.
+	 */
+	void		vehicle_attitude_setpoint_poll();
+
+	/**
+	 * Check for rates setpoint updates.
+	 */
+	void		vehicle_rates_setpoint_poll();
+
+	/**
+	 * Check for arming status updates.
+	 */
+	void		arming_status_poll();
+
+	/**
+	 * Attitude controller.
+	 */
+	void		control_attitude(float dt);
+
+	/**
+	 * Attitude rates controller.
+	 */
+	void		control_attitude_rates(float dt);
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main attitude control task.
+	 */
+	void		task_main();
+};
+
+namespace mc_att_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MulticopterAttitudeControl	*g_control;
+}
+
+MulticopterAttitudeControl::MulticopterAttitudeControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+
+/* subscriptions */
+	_v_att_sub(-1),
+	_v_att_sp_sub(-1),
+	_v_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sp_sub(-1),
+	_armed_sub(-1),
+
+/* publications */
+	_att_sp_pub(-1),
+	_v_rates_sp_pub(-1),
+	_actuators_0_pub(-1),
+
+	_actuators_0_circuit_breaker_enabled(false),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+
+{
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_actuators, 0, sizeof(_actuators));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params.att_p.zero();
+	_params.rate_p.zero();
+	_params.rate_i.zero();
+	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_params.man_roll_max = 0.0f;
+	_params.man_pitch_max = 0.0f;
+	_params.man_yaw_max = 0.0f;
+	_params.acro_rate_max.zero();
+
+	_rates_prev.zero();
+	_rates_sp.zero();
+	_rates_int.zero();
+	_thrust_sp = 0.0f;
+	_att_control.zero();
+
+	_I.identity();
+
+	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
+	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
+	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
+	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
+	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
+	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
+	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
+	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
+	_params_handles.yaw_p			=	param_find("MC_YAW_P");
+	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
+	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
+	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
+	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
+	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
+	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
+	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
+	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
+	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
+	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
+	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+MulticopterAttitudeControl::~MulticopterAttitudeControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	mc_att_control::g_control = nullptr;
+}
+
+int
+MulticopterAttitudeControl::parameters_update()
+{
+	float v;
+
+	/* roll gains */
+	param_get(_params_handles.roll_p, &v);
+	_params.att_p(0) = v;
+	param_get(_params_handles.roll_rate_p, &v);
+	_params.rate_p(0) = v;
+	param_get(_params_handles.roll_rate_i, &v);
+	_params.rate_i(0) = v;
+	param_get(_params_handles.roll_rate_d, &v);
+	_params.rate_d(0) = v;
+
+	/* pitch gains */
+	param_get(_params_handles.pitch_p, &v);
+	_params.att_p(1) = v;
+	param_get(_params_handles.pitch_rate_p, &v);
+	_params.rate_p(1) = v;
+	param_get(_params_handles.pitch_rate_i, &v);
+	_params.rate_i(1) = v;
+	param_get(_params_handles.pitch_rate_d, &v);
+	_params.rate_d(1) = v;
+
+	/* yaw gains */
+	param_get(_params_handles.yaw_p, &v);
+	_params.att_p(2) = v;
+	param_get(_params_handles.yaw_rate_p, &v);
+	_params.rate_p(2) = v;
+	param_get(_params_handles.yaw_rate_i, &v);
+	_params.rate_i(2) = v;
+	param_get(_params_handles.yaw_rate_d, &v);
+	_params.rate_d(2) = v;
+
+	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
+	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
+
+	/* manual control scale */
+	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
+	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
+	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
+	_params.man_roll_max = math::radians(_params.man_roll_max);
+	_params.man_pitch_max = math::radians(_params.man_pitch_max);
+	_params.man_yaw_max = math::radians(_params.man_yaw_max);
+
+	/* acro control scale */
+	param_get(_params_handles.acro_roll_max, &v);
+	_params.acro_rate_max(0) = math::radians(v);
+	param_get(_params_handles.acro_pitch_max, &v);
+	_params.acro_rate_max(1) = math::radians(v);
+	param_get(_params_handles.acro_yaw_max, &v);
+	_params.acro_rate_max(2) = math::radians(v);
+
+	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
+
+	return OK;
+}
+
+void
+MulticopterAttitudeControl::parameter_update_poll()
+{
+	bool updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_control_mode_poll()
+{
+	bool updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_v_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_manual_poll()
+{
+	bool updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_control_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_rates_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::arming_status_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+	}
+}
+
+/*
+ * Attitude controller.
+ * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
+ * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
+ */
+void
+MulticopterAttitudeControl::control_attitude(float dt)
+{
+	float yaw_sp_move_rate = 0.0f;
+	bool publish_att_sp = false;
+
+	if (_v_control_mode.flag_control_manual_enabled) {
+		/* manual input, set or modify attitude setpoint */
+
+		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
+			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
+			vehicle_attitude_setpoint_poll();
+		}
+
+		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+			/* pass throttle directly if not in altitude stabilized mode */
+			_v_att_sp.thrust = _manual_control_sp.z;
+			publish_att_sp = true;
+		}
+
+		if (!_armed.armed) {
+			/* reset yaw setpoint when disarmed */
+			_reset_yaw_sp = true;
+		}
+
+		/* move yaw setpoint in all modes */
+		if (_v_att_sp.thrust < 0.1f) {
+			// TODO
+			//if (_status.condition_landed) {
+			/* reset yaw setpoint if on ground */
+			//	reset_yaw_sp = true;
+			//}
+		} else {
+			/* move yaw setpoint */
+			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
+			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			if (yaw_offs < - yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+
+			} else if (yaw_offs > yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+			}
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		/* reset yaw setpint to current position if needed */
+		if (_reset_yaw_sp) {
+			_reset_yaw_sp = false;
+			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		if (!_v_control_mode.flag_control_velocity_enabled) {
+			/* update attitude setpoint if not in position control mode */
+			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+	} else {
+		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
+		vehicle_attitude_setpoint_poll();
+
+		/* reset yaw setpoint after non-manual control mode */
+		_reset_yaw_sp = true;
+	}
+
+	_thrust_sp = _v_att_sp.thrust;
+
+	/* construct attitude setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+
+	if (_v_att_sp.R_valid) {
+		/* rotation matrix in _att_sp is valid, use it */
+		R_sp.set(&_v_att_sp.R_body[0][0]);
+
+	} else {
+		/* rotation matrix in _att_sp is not valid, use euler angles instead */
+		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+
+		/* copy rotation matrix back to setpoint struct */
+		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
+		_v_att_sp.R_valid = true;
+	}
+
+	/* publish the attitude setpoint if needed */
+	if (publish_att_sp) {
+		_v_att_sp.timestamp = hrt_absolute_time();
+
+		if (_att_sp_pub > 0) {
+			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
+
+		} else {
+			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
+		}
+	}
+
+	/* rotation matrix for current state */
+	math::Matrix<3, 3> R;
+	R.set(_v_att.R);
+
+	/* all input data is ready, run controller itself */
+
+	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
+	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+
+	/* axis and sin(angle) of desired rotation */
+	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
+
+	/* calculate angle error */
+	float e_R_z_sin = e_R.length();
+	float e_R_z_cos = R_z * R_sp_z;
+
+	/* calculate weight for yaw control */
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
+
+	/* calculate rotation matrix after roll/pitch only rotation */
+	math::Matrix<3, 3> R_rp;
+
+	if (e_R_z_sin > 0.0f) {
+		/* get axis-angle representation */
+		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
+
+		e_R = e_R_z_axis * e_R_z_angle;
+
+		/* cross product matrix for e_R_axis */
+		math::Matrix<3, 3> e_R_cp;
+		e_R_cp.zero();
+		e_R_cp(0, 1) = -e_R_z_axis(2);
+		e_R_cp(0, 2) = e_R_z_axis(1);
+		e_R_cp(1, 0) = e_R_z_axis(2);
+		e_R_cp(1, 2) = -e_R_z_axis(0);
+		e_R_cp(2, 0) = -e_R_z_axis(1);
+		e_R_cp(2, 1) = e_R_z_axis(0);
+
+		/* rotation matrix for roll/pitch only rotation */
+		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+
+	} else {
+		/* zero roll/pitch rotation */
+		R_rp = R;
+	}
+
+	/* R_rp and R_sp has the same Z axis, calculate yaw error */
+	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
+
+	if (e_R_z_cos < 0.0f) {
+		/* for large thrust vector rotations use another rotation method:
+		 * calculate angle and axis for R -> R_sp rotation directly */
+		math::Quaternion q;
+		q.from_dcm(R.transposed() * R_sp);
+		math::Vector<3> e_R_d = q.imag();
+		e_R_d.normalize();
+		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+		/* use fusion of Z axis based rotation and direct rotation */
+		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+	}
+
+	/* calculate angular rates setpoint */
+	_rates_sp = _params.att_p.emult(e_R);
+
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
+
+	/* feed forward yaw setpoint rate */
+	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+/*
+ * Attitude rates controller.
+ * Input: '_rates_sp' vector, '_thrust_sp'
+ * Output: '_att_control' vector
+ */
+void
+MulticopterAttitudeControl::control_attitude_rates(float dt)
+{
+	/* reset integral if disarmed */
+	if (!_armed.armed) {
+		_rates_int.zero();
+	}
+
+	/* current body angular rates */
+	math::Vector<3> rates;
+	rates(0) = _v_att.rollspeed;
+	rates(1) = _v_att.pitchspeed;
+	rates(2) = _v_att.yawspeed;
+
+	/* angular rates error */
+	math::Vector<3> rates_err = _rates_sp - rates;
+	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+	_rates_prev = rates;
+
+	/* update integral only if not saturated on low limit */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+		for (int i = 0; i < 3; i++) {
+			if (fabsf(_att_control(i)) < _thrust_sp) {
+				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
+
+				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					_rates_int(i) = rate_i;
+				}
+			}
+		}
+	}
+}
+
+void
+MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	mc_att_control::g_control->task_main();
+}
+
+void
+MulticopterAttitudeControl::task_main()
+{
+	warnx("started");
+	fflush(stdout);
+
+	/*
+	 * do subscriptions
+	 */
+	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	/* initialize parameters cache */
+	parameters_update();
+
+	/* wakeup source: vehicle attitude */
+	struct pollfd fds[1];
+
+	fds[0].fd = _v_att_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+
+		/* 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 */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* run controller on attitude changes */
+		if (fds[0].revents & POLLIN) {
+			static uint64_t last_run = 0;
+			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
+			last_run = hrt_absolute_time();
+
+			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
+			if (dt < 0.002f) {
+				dt = 0.002f;
+
+			} else if (dt > 0.02f) {
+				dt = 0.02f;
+			}
+
+			/* copy attitude topic */
+			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+
+			/* check for updates in other topics */
+			parameter_update_poll();
+			vehicle_control_mode_poll();
+			arming_status_poll();
+			vehicle_manual_poll();
+
+			if (_v_control_mode.flag_control_attitude_enabled) {
+				control_attitude(dt);
+
+				/* publish attitude rates setpoint */
+				_v_rates_sp.roll = _rates_sp(0);
+				_v_rates_sp.pitch = _rates_sp(1);
+				_v_rates_sp.yaw = _rates_sp(2);
+				_v_rates_sp.thrust = _thrust_sp;
+				_v_rates_sp.timestamp = hrt_absolute_time();
+
+				if (_v_rates_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+				} else {
+					_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+				}
+
+			} else {
+				/* attitude controller disabled, poll rates setpoint topic */
+				if (_v_control_mode.flag_control_manual_enabled) {
+					/* manual rates control - ACRO mode */
+					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
+					_thrust_sp = _manual_control_sp.z;
+
+					/* reset yaw setpoint after ACRO */
+					_reset_yaw_sp = true;
+
+					/* publish attitude rates setpoint */
+					_v_rates_sp.roll = _rates_sp(0);
+					_v_rates_sp.pitch = _rates_sp(1);
+					_v_rates_sp.yaw = _rates_sp(2);
+					_v_rates_sp.thrust = _thrust_sp;
+					_v_rates_sp.timestamp = hrt_absolute_time();
+
+					if (_v_rates_sp_pub > 0) {
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+					} else {
+						_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+					}
+
+				} else {
+					/* attitude controller disabled, poll rates setpoint topic */
+					vehicle_rates_setpoint_poll();
+					_rates_sp(0) = _v_rates_sp.roll;
+					_rates_sp(1) = _v_rates_sp.pitch;
+					_rates_sp(2) = _v_rates_sp.yaw;
+					_thrust_sp = _v_rates_sp.thrust;
+				}
+			}
+
+			if (_v_control_mode.flag_control_rates_enabled) {
+				control_attitude_rates(dt);
+
+				/* publish actuator controls */
+				_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+				_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+				_actuators.timestamp = hrt_absolute_time();
+
+				if (!_actuators_0_circuit_breaker_enabled) {
+					if (_actuators_0_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+
+					} else {
+						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+					}
+				}
+			}
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exit");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+MulticopterAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("mc_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int mc_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: mc_att_control {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (mc_att_control::g_control != nullptr)
+			errx(1, "already running");
+
+		mc_att_control::g_control = new MulticopterAttitudeControl;
+
+		if (mc_att_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != mc_att_control::g_control->start()) {
+			delete mc_att_control::g_control;
+			mc_att_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (mc_att_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete mc_att_control::g_control;
+		mc_att_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (mc_att_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
new file mode 100644
index 000000000..819847b40
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -0,0 +1,246 @@
+/****************************************************************************
+ *
+ *   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 mc_att_control_params.c
+ * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Roll P gain
+ *
+ * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
+
+/**
+ * Roll rate P gain
+ *
+ * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
+
+/**
+ * Roll rate I gain
+ *
+ * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
+
+/**
+ * Roll rate D gain
+ *
+ * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
+
+/**
+ * Pitch P gain
+ *
+ * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
+
+/**
+ * Pitch rate P gain
+ *
+ * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
+
+/**
+ * Pitch rate I gain
+ *
+ * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
+
+/**
+ * Pitch rate D gain
+ *
+ * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
+
+/**
+ * Yaw P gain
+ *
+ * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
+
+/**
+ * Yaw rate P gain
+ *
+ * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
+
+/**
+ * Yaw rate I gain
+ *
+ * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+
+/**
+ * Yaw rate D gain
+ *
+ * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
+
+/**
+ * Yaw feed forward
+ *
+ * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
+
+/**
+ * Max yaw rate
+ *
+ * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
+
+/**
+ * Max manual roll
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
+
+/**
+ * Max manual pitch
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
+
+/**
+ * Max manual yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
+
+/**
+ * Max acro roll rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
+
+/**
+ * Max acro pitch rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
+
+/**
+ * Max acro yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/mc_att_control/module.mk
index bc4b48fb4..64b876f69 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/mc_att_control/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 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
@@ -32,11 +32,10 @@
 ############################################################################
 
 #
-# Build multirotor position control
+# Multirotor attitude controller (vector based, no Euler singularities)
 #
 
-MODULE_COMMAND	= multirotor_pos_control
+MODULE_COMMAND	= mc_att_control
 
-SRCS		= multirotor_pos_control.c \
-		  multirotor_pos_control_params.c \
-		  thrust_pid.c
+SRCS		= mc_att_control_main.cpp \
+			  mc_att_control_params.c
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
new file mode 100644
index 000000000..9b0f69226
--- /dev/null
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -0,0 +1,1213 @@
+/****************************************************************************
+ *
+ *   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 mc_pos_control_main.cpp
+ * Multicopter position controller.
+ *
+ * The controller has two loops: P loop for position error and PID loop for velocity error.
+ * Output of velocity controller is thrust vector that splitted to thrust direction
+ * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <mavlink/mavlink_log.h>
+
+#define TILT_COS_MAX	0.7f
+#define SIGMA			0.000001f
+
+/**
+ * Multicopter position control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
+
+class MulticopterPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterPositionControl();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~MulticopterPositionControl();
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool		_task_should_exit;		/**< if true, task should exit */
+	int		_control_task;			/**< task handle for task */
+	int		_mavlink_fd;			/**< mavlink fd */
+
+	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_att_sp_sub;			/**< vehicle attitude setpoint */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
+	int		_params_sub;			/**< notification of parameter updates */
+	int		_manual_sub;			/**< notification of manual control updates */
+	int		_arming_sub;			/**< arming status of outputs */
+	int		_local_pos_sub;			/**< vehicle local position */
+	int		_pos_sp_triplet_sub;	/**< position setpoint triplet */
+	int		_local_pos_sp_sub;		/**< offboard local position setpoint */
+	int		_global_vel_sp_sub;		/**< offboard global velocity setpoint */
+
+	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
+	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
+	struct vehicle_control_mode_s			_control_mode;	/**< vehicle control mode */
+	struct actuator_armed_s				_arming;		/**< actuator arming status */
+	struct vehicle_local_position_s			_local_pos;		/**< vehicle local position */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< vehicle global position setpoint triplet */
+	struct vehicle_local_position_setpoint_s	_local_pos_sp;		/**< vehicle local position setpoint */
+	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
+
+
+	struct {
+		param_t thr_min;
+		param_t thr_max;
+		param_t z_p;
+		param_t z_vel_p;
+		param_t z_vel_i;
+		param_t z_vel_d;
+		param_t z_vel_max;
+		param_t z_ff;
+		param_t xy_p;
+		param_t xy_vel_p;
+		param_t xy_vel_i;
+		param_t xy_vel_d;
+		param_t xy_vel_max;
+		param_t xy_ff;
+		param_t tilt_max_air;
+		param_t land_speed;
+		param_t tilt_max_land;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		float thr_min;
+		float thr_max;
+		float tilt_max_air;
+		float land_speed;
+		float tilt_max_land;
+
+		math::Vector<3> pos_p;
+		math::Vector<3> vel_p;
+		math::Vector<3> vel_i;
+		math::Vector<3> vel_d;
+		math::Vector<3> vel_ff;
+		math::Vector<3> vel_max;
+		math::Vector<3> sp_offs_max;
+	}		_params;
+
+	struct map_projection_reference_s _ref_pos;
+	float _ref_alt;
+	hrt_abstime _ref_timestamp;
+
+	bool _reset_pos_sp;
+	bool _reset_alt_sp;
+
+	math::Vector<3> _pos;
+	math::Vector<3> _pos_sp;
+	math::Vector<3> _vel;
+	math::Vector<3> _vel_sp;
+	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update(bool force);
+
+	/**
+	 * Update control outputs
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in subscribed topics.
+	 */
+	void		poll_subscriptions();
+
+	static float	scale_control(float ctl, float end, float dz);
+
+	/**
+	 * Update reference for local position projection
+	 */
+	void		update_ref();
+	/**
+	 * Reset position setpoint to current position
+	 */
+	void		reset_pos_sp();
+
+	/**
+	 * Reset altitude setpoint to current altitude
+	 */
+	void		reset_alt_sp();
+
+	/**
+	 * Select between barometric and global (AMSL) altitudes
+	 */
+	void		select_alt(bool global);
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main sensor collection task.
+	 */
+	void		task_main();
+};
+
+namespace pos_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MulticopterPositionControl	*g_control;
+}
+
+MulticopterPositionControl::MulticopterPositionControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+	_mavlink_fd(-1),
+
+/* subscriptions */
+	_att_sub(-1),
+	_att_sp_sub(-1),
+	_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_sub(-1),
+	_arming_sub(-1),
+	_local_pos_sub(-1),
+	_pos_sp_triplet_sub(-1),
+	_global_vel_sp_sub(-1),
+
+/* publications */
+	_att_sp_pub(-1),
+	_local_pos_sp_pub(-1),
+	_global_vel_sp_pub(-1),
+
+	_ref_alt(0.0f),
+	_ref_timestamp(0),
+
+	_reset_pos_sp(true),
+	_reset_alt_sp(true)
+{
+	memset(&_att, 0, sizeof(_att));
+	memset(&_att_sp, 0, sizeof(_att_sp));
+	memset(&_manual, 0, sizeof(_manual));
+	memset(&_control_mode, 0, sizeof(_control_mode));
+	memset(&_arming, 0, sizeof(_arming));
+	memset(&_local_pos, 0, sizeof(_local_pos));
+	memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
+	memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
+	memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
+
+	memset(&_ref_pos, 0, sizeof(_ref_pos));
+
+	_params.pos_p.zero();
+	_params.vel_p.zero();
+	_params.vel_i.zero();
+	_params.vel_d.zero();
+	_params.vel_max.zero();
+	_params.vel_ff.zero();
+	_params.sp_offs_max.zero();
+
+	_pos.zero();
+	_pos_sp.zero();
+	_vel.zero();
+	_vel_sp.zero();
+	_vel_prev.zero();
+
+	_params_handles.thr_min		= param_find("MPC_THR_MIN");
+	_params_handles.thr_max		= param_find("MPC_THR_MAX");
+	_params_handles.z_p			= param_find("MPC_Z_P");
+	_params_handles.z_vel_p		= param_find("MPC_Z_VEL_P");
+	_params_handles.z_vel_i		= param_find("MPC_Z_VEL_I");
+	_params_handles.z_vel_d		= param_find("MPC_Z_VEL_D");
+	_params_handles.z_vel_max	= param_find("MPC_Z_VEL_MAX");
+	_params_handles.z_ff		= param_find("MPC_Z_FF");
+	_params_handles.xy_p		= param_find("MPC_XY_P");
+	_params_handles.xy_vel_p	= param_find("MPC_XY_VEL_P");
+	_params_handles.xy_vel_i	= param_find("MPC_XY_VEL_I");
+	_params_handles.xy_vel_d	= param_find("MPC_XY_VEL_D");
+	_params_handles.xy_vel_max	= param_find("MPC_XY_VEL_MAX");
+	_params_handles.xy_ff		= param_find("MPC_XY_FF");
+	_params_handles.tilt_max_air	= param_find("MPC_TILTMAX_AIR");
+	_params_handles.land_speed	= param_find("MPC_LAND_SPEED");
+	_params_handles.tilt_max_land	= param_find("MPC_TILTMAX_LND");
+
+	/* fetch initial parameter values */
+	parameters_update(true);
+}
+
+MulticopterPositionControl::~MulticopterPositionControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	pos_control::g_control = nullptr;
+}
+
+int
+MulticopterPositionControl::parameters_update(bool force)
+{
+	bool updated;
+	struct parameter_update_s param_upd;
+
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
+	}
+
+	if (updated || force) {
+		param_get(_params_handles.thr_min, &_params.thr_min);
+		param_get(_params_handles.thr_max, &_params.thr_max);
+		param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
+		_params.tilt_max_air = math::radians(_params.tilt_max_air);
+		param_get(_params_handles.land_speed, &_params.land_speed);
+		param_get(_params_handles.tilt_max_land, &_params.tilt_max_land);
+		_params.tilt_max_land = math::radians(_params.tilt_max_land);
+
+		float v;
+		param_get(_params_handles.xy_p, &v);
+		_params.pos_p(0) = v;
+		_params.pos_p(1) = v;
+		param_get(_params_handles.z_p, &v);
+		_params.pos_p(2) = v;
+		param_get(_params_handles.xy_vel_p, &v);
+		_params.vel_p(0) = v;
+		_params.vel_p(1) = v;
+		param_get(_params_handles.z_vel_p, &v);
+		_params.vel_p(2) = v;
+		param_get(_params_handles.xy_vel_i, &v);
+		_params.vel_i(0) = v;
+		_params.vel_i(1) = v;
+		param_get(_params_handles.z_vel_i, &v);
+		_params.vel_i(2) = v;
+		param_get(_params_handles.xy_vel_d, &v);
+		_params.vel_d(0) = v;
+		_params.vel_d(1) = v;
+		param_get(_params_handles.z_vel_d, &v);
+		_params.vel_d(2) = v;
+		param_get(_params_handles.xy_vel_max, &v);
+		_params.vel_max(0) = v;
+		_params.vel_max(1) = v;
+		param_get(_params_handles.z_vel_max, &v);
+		_params.vel_max(2) = v;
+		param_get(_params_handles.xy_ff, &v);
+		_params.vel_ff(0) = v;
+		_params.vel_ff(1) = v;
+		param_get(_params_handles.z_ff, &v);
+		_params.vel_ff(2) = v;
+
+		_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
+	}
+
+	return OK;
+}
+
+void
+MulticopterPositionControl::poll_subscriptions()
+{
+	bool updated;
+
+	orb_check(_att_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+	}
+
+	orb_check(_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+	}
+
+	orb_check(_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+	}
+
+	orb_check(_manual_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
+	}
+
+	orb_check(_arming_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
+	}
+
+	orb_check(_local_pos_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+	}
+}
+
+float
+MulticopterPositionControl::scale_control(float ctl, float end, float dz)
+{
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+
+	} else {
+		return 0.0f;
+	}
+}
+
+void
+MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
+{
+	pos_control::g_control->task_main();
+}
+
+void
+MulticopterPositionControl::update_ref()
+{
+	if (_local_pos.ref_timestamp != _ref_timestamp) {
+		double lat_sp, lon_sp;
+		float alt_sp = 0.0f;
+
+		if (_ref_timestamp != 0) {
+			/* calculate current position setpoint in global frame */
+			map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
+			alt_sp = _ref_alt - _pos_sp(2);
+		}
+
+		/* update local projection reference */
+		map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
+		_ref_alt = _local_pos.ref_alt;
+
+		if (_ref_timestamp != 0) {
+			/* reproject position setpoint to new reference */
+			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
+			_pos_sp(2) = -(alt_sp - _ref_alt);
+		}
+
+		_ref_timestamp = _local_pos.ref_timestamp;
+	}
+}
+
+void
+MulticopterPositionControl::reset_pos_sp()
+{
+	if (_reset_pos_sp) {
+		_reset_pos_sp = false;
+		_pos_sp(0) = _pos(0);
+		_pos_sp(1) = _pos(1);
+		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
+	}
+}
+
+void
+MulticopterPositionControl::reset_alt_sp()
+{
+	if (_reset_alt_sp) {
+		_reset_alt_sp = false;
+		_pos_sp(2) = _pos(2);
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
+	}
+}
+
+void
+MulticopterPositionControl::task_main()
+{
+	warnx("started");
+
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(_mavlink_fd, "[mpc] started");
+
+	/*
+	 * do subscriptions
+	 */
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
+
+
+	parameters_update(true);
+
+	/* initialize values of critical structs until first regular update */
+	_arming.armed = false;
+
+	/* get an initial update for all sensor and status data */
+	poll_subscriptions();
+
+	bool reset_int_z = true;
+	bool reset_int_z_manual = false;
+	bool reset_int_xy = true;
+	bool was_armed = false;
+
+	hrt_abstime t_prev = 0;
+
+	const float alt_ctl_dz = 0.2f;
+
+	math::Vector<3> sp_move_rate;
+	sp_move_rate.zero();
+
+	float yaw_sp_move_rate;
+
+	math::Vector<3> thrust_int;
+	thrust_int.zero();
+	math::Matrix<3, 3> R;
+	R.identity();
+
+	/* wakeup source */
+	struct pollfd fds[1];
+
+	fds[0].fd = _local_pos_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+		/* wait for up to 500ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0) {
+			continue;
+		}
+
+		/* this is undesirable but not much we can do */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		poll_subscriptions();
+		parameters_update(false);
+
+		hrt_abstime t = hrt_absolute_time();
+		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
+		t_prev = t;
+
+		if (_control_mode.flag_armed && !was_armed) {
+			/* reset setpoints and integrals on arming */
+			_reset_pos_sp = true;
+			_reset_alt_sp = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+		}
+
+		was_armed = _control_mode.flag_armed;
+
+		update_ref();
+
+		if (_control_mode.flag_control_altitude_enabled ||
+		    _control_mode.flag_control_position_enabled ||
+		    _control_mode.flag_control_climb_rate_enabled ||
+		    _control_mode.flag_control_velocity_enabled) {
+
+			_pos(0) = _local_pos.x;
+			_pos(1) = _local_pos.y;
+			_pos(2) = _local_pos.z;
+
+			_vel(0) = _local_pos.vx;
+			_vel(1) = _local_pos.vy;
+			_vel(2) = _local_pos.vz;
+
+			sp_move_rate.zero();
+
+			/* select control source */
+			if (_control_mode.flag_control_manual_enabled) {
+				/* manual control */
+				if (_control_mode.flag_control_altitude_enabled) {
+					/* reset alt setpoint to current altitude if needed */
+					reset_alt_sp();
+
+					/* move altitude setpoint with throttle stick */
+					sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
+				}
+
+				if (_control_mode.flag_control_position_enabled) {
+					/* reset position setpoint to current position if needed */
+					reset_pos_sp();
+
+					/* move position setpoint with roll/pitch stick */
+					sp_move_rate(0) = _manual.x;
+					sp_move_rate(1) = _manual.y;
+				}
+
+				/* limit setpoint move rate */
+				float sp_move_norm = sp_move_rate.length();
+
+				if (sp_move_norm > 1.0f) {
+					sp_move_rate /= sp_move_norm;
+				}
+
+				/* scale to max speed and rotate around yaw */
+				math::Matrix<3, 3> R_yaw_sp;
+				R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
+
+				/* move position setpoint */
+				_pos_sp += sp_move_rate * dt;
+
+				/* check if position setpoint is too far from actual position */
+				math::Vector<3> pos_sp_offs;
+				pos_sp_offs.zero();
+
+				if (_control_mode.flag_control_position_enabled) {
+					pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+					pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+				}
+
+				if (_control_mode.flag_control_altitude_enabled) {
+					pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+				}
+
+				float pos_sp_offs_norm = pos_sp_offs.length();
+
+				if (pos_sp_offs_norm > 1.0f) {
+					pos_sp_offs /= pos_sp_offs_norm;
+					_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+				}
+
+			} else if (_control_mode.flag_control_offboard_enabled) {
+				/* Offboard control */
+				bool updated;
+				orb_check(_pos_sp_triplet_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+				}
+
+				if (_pos_sp_triplet.current.valid) {
+
+					if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
+
+						_pos_sp(0) = _pos_sp_triplet.current.x;
+						_pos_sp(1) = _pos_sp_triplet.current.y;
+						_pos_sp(2) = _pos_sp_triplet.current.z;
+						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+
+					} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
+						/* reset position setpoint to current position if needed */
+						reset_pos_sp();
+						/* move position setpoint with roll/pitch stick */
+						sp_move_rate(0) = _pos_sp_triplet.current.vx;
+						sp_move_rate(1) = _pos_sp_triplet.current.vy;
+						yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
+						_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
+					}
+
+					if (_control_mode.flag_control_altitude_enabled) {
+						/* reset alt setpoint to current altitude if needed */
+						reset_alt_sp();
+
+						/* move altitude setpoint with throttle stick */
+						sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
+					}
+
+					/* limit setpoint move rate */
+					float sp_move_norm = sp_move_rate.length();
+
+					if (sp_move_norm > 1.0f) {
+						sp_move_rate /= sp_move_norm;
+					}
+
+					/* scale to max speed and rotate around yaw */
+					math::Matrix<3, 3> R_yaw_sp;
+					R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+					sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
+
+					/* move position setpoint */
+					_pos_sp += sp_move_rate * dt;
+
+					/* check if position setpoint is too far from actual position */
+					math::Vector<3> pos_sp_offs;
+					pos_sp_offs.zero();
+
+					if (_control_mode.flag_control_position_enabled) {
+						pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+						pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+					}
+
+					if (_control_mode.flag_control_altitude_enabled) {
+						pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+					}
+
+					float pos_sp_offs_norm = pos_sp_offs.length();
+
+					if (pos_sp_offs_norm > 1.0f) {
+						pos_sp_offs /= pos_sp_offs_norm;
+						_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+					}
+
+				} else {
+					reset_pos_sp();
+					reset_alt_sp();
+				}
+
+			} else {
+				/* AUTO */
+				bool updated;
+				orb_check(_pos_sp_triplet_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+				}
+
+				if (_pos_sp_triplet.current.valid) {
+					/* in case of interrupted mission don't go to waypoint but stay at current position */
+					_reset_pos_sp = true;
+					_reset_alt_sp = true;
+
+					/* project setpoint to local frame */
+					map_projection_project(&_ref_pos,
+							       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
+							       &_pos_sp.data[0], &_pos_sp.data[1]);
+					_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
+
+					/* update yaw setpoint if needed */
+					if (isfinite(_pos_sp_triplet.current.yaw)) {
+						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+					}
+
+				} else {
+					/* no waypoint, loiter, reset position setpoint if needed */
+					reset_pos_sp();
+					reset_alt_sp();
+				}
+			}
+
+			/* fill local position setpoint */
+			_local_pos_sp.x = _pos_sp(0);
+			_local_pos_sp.y = _pos_sp(1);
+			_local_pos_sp.z = _pos_sp(2);
+			_local_pos_sp.yaw = _att_sp.yaw_body;
+
+			/* publish local position setpoint */
+			if (_local_pos_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
+
+			} else {
+				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
+			}
+
+
+			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
+				/* idle state, don't run controller and set zero thrust */
+				R.identity();
+				memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+				_att_sp.R_valid = true;
+
+				_att_sp.roll_body = 0.0f;
+				_att_sp.pitch_body = 0.0f;
+				_att_sp.yaw_body = _att.yaw;
+				_att_sp.thrust = 0.0f;
+
+				_att_sp.timestamp = hrt_absolute_time();
+
+				/* publish attitude setpoint */
+				if (_att_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+				} else {
+					_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+				}
+
+			} else {
+				/* run position & altitude controllers, calculate velocity setpoint */
+				math::Vector<3> pos_err = _pos_sp - _pos;
+
+				_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
+
+				if (!_control_mode.flag_control_altitude_enabled) {
+					_reset_alt_sp = true;
+					_vel_sp(2) = 0.0f;
+				}
+
+				if (!_control_mode.flag_control_position_enabled) {
+					_reset_pos_sp = true;
+					_vel_sp(0) = 0.0f;
+					_vel_sp(1) = 0.0f;
+				}
+
+				/* use constant descend rate when landing, ignore altitude setpoint */
+				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+					_vel_sp(2) = _params.land_speed;
+				}
+
+
+				if (!_control_mode.flag_control_manual_enabled) {
+					/* limit 3D speed only in non-manual modes */
+					float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
+
+					if (vel_sp_norm > 1.0f) {
+						_vel_sp /= vel_sp_norm;
+					}
+				}
+
+				_global_vel_sp.vx = _vel_sp(0);
+				_global_vel_sp.vy = _vel_sp(1);
+				_global_vel_sp.vz = _vel_sp(2);
+
+				/* publish velocity setpoint */
+				if (_global_vel_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
+
+				} else {
+					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
+				}
+
+				if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
+					/* reset integrals if needed */
+					if (_control_mode.flag_control_climb_rate_enabled) {
+						if (reset_int_z) {
+							reset_int_z = false;
+							float i = _params.thr_min;
+
+							if (reset_int_z_manual) {
+								i = _manual.z;
+
+								if (i < _params.thr_min) {
+									i = _params.thr_min;
+
+								} else if (i > _params.thr_max) {
+									i = _params.thr_max;
+								}
+							}
+
+							thrust_int(2) = -i;
+						}
+
+					} else {
+						reset_int_z = true;
+					}
+
+					if (_control_mode.flag_control_velocity_enabled) {
+						if (reset_int_xy) {
+							reset_int_xy = false;
+							thrust_int(0) = 0.0f;
+							thrust_int(1) = 0.0f;
+						}
+
+					} else {
+						reset_int_xy = true;
+					}
+
+					/* velocity error */
+					math::Vector<3> vel_err = _vel_sp - _vel;
+
+					/* derivative of velocity error, not includes setpoint acceleration */
+					math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+					_vel_prev = _vel;
+
+					/* thrust vector in NED frame */
+					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
+
+					if (!_control_mode.flag_control_velocity_enabled) {
+						thrust_sp(0) = 0.0f;
+						thrust_sp(1) = 0.0f;
+					}
+
+					if (!_control_mode.flag_control_climb_rate_enabled) {
+						thrust_sp(2) = 0.0f;
+					}
+
+					/* limit thrust vector and check for saturation */
+					bool saturation_xy = false;
+					bool saturation_z = false;
+
+					/* limit min lift */
+					float thr_min = _params.thr_min;
+
+					if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
+						/* don't allow downside thrust direction in manual attitude mode */
+						thr_min = 0.0f;
+					}
+
+					float tilt_max = _params.tilt_max_air;
+
+					/* adjust limits for landing mode */
+					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
+					    _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+						/* limit max tilt and min lift when landing */
+						tilt_max = _params.tilt_max_land;
+
+						if (thr_min < 0.0f) {
+							thr_min = 0.0f;
+						}
+					}
+
+					/* limit min lift */
+					if (-thrust_sp(2) < thr_min) {
+						thrust_sp(2) = -thr_min;
+						saturation_z = true;
+					}
+
+					if (_control_mode.flag_control_velocity_enabled) {
+						/* limit max tilt */
+						if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
+							/* absolute horizontal thrust */
+							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+
+							if (thrust_sp_xy_len > 0.01f) {
+								/* max horizontal thrust for given vertical thrust*/
+								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
+
+								if (thrust_sp_xy_len > thrust_xy_max) {
+									float k = thrust_xy_max / thrust_sp_xy_len;
+									thrust_sp(0) *= k;
+									thrust_sp(1) *= k;
+									saturation_xy = true;
+								}
+							}
+						}
+
+					} else {
+						/* thrust compensation for altitude only control mode */
+						float att_comp;
+
+						if (_att.R[2][2] > TILT_COS_MAX) {
+							att_comp = 1.0f / _att.R[2][2];
+
+						} else if (_att.R[2][2] > 0.0f) {
+							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
+							saturation_z = true;
+
+						} else {
+							att_comp = 1.0f;
+							saturation_z = true;
+						}
+
+						thrust_sp(2) *= att_comp;
+					}
+
+					/* limit max thrust */
+					float thrust_abs = thrust_sp.length();
+
+					if (thrust_abs > _params.thr_max) {
+						if (thrust_sp(2) < 0.0f) {
+							if (-thrust_sp(2) > _params.thr_max) {
+								/* thrust Z component is too large, limit it */
+								thrust_sp(0) = 0.0f;
+								thrust_sp(1) = 0.0f;
+								thrust_sp(2) = -_params.thr_max;
+								saturation_xy = true;
+								saturation_z = true;
+
+							} else {
+								/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
+								float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
+								float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+								float k = thrust_xy_max / thrust_xy_abs;
+								thrust_sp(0) *= k;
+								thrust_sp(1) *= k;
+								saturation_xy = true;
+							}
+
+						} else {
+							/* Z component is negative, going down, simply limit thrust vector */
+							float k = _params.thr_max / thrust_abs;
+							thrust_sp *= k;
+							saturation_xy = true;
+							saturation_z = true;
+						}
+
+						thrust_abs = _params.thr_max;
+					}
+
+					/* update integrals */
+					if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
+						thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
+						thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
+					}
+
+					if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
+						thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
+
+						/* protection against flipping on ground when landing */
+						if (thrust_int(2) > 0.0f) {
+							thrust_int(2) = 0.0f;
+						}
+					}
+
+					/* calculate attitude setpoint from thrust vector */
+					if (_control_mode.flag_control_velocity_enabled) {
+						/* desired body_z axis = -normalize(thrust_vector) */
+						math::Vector<3> body_x;
+						math::Vector<3> body_y;
+						math::Vector<3> body_z;
+
+						if (thrust_abs > SIGMA) {
+							body_z = -thrust_sp / thrust_abs;
+
+						} else {
+							/* no thrust, set Z axis to safe value */
+							body_z.zero();
+							body_z(2) = 1.0f;
+						}
+
+						/* vector of desired yaw direction in XY plane, rotated by PI/2 */
+						math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
+
+						if (fabsf(body_z(2)) > SIGMA) {
+							/* desired body_x axis, orthogonal to body_z */
+							body_x = y_C % body_z;
+
+							/* keep nose to front while inverted upside down */
+							if (body_z(2) < 0.0f) {
+								body_x = -body_x;
+							}
+
+							body_x.normalize();
+
+						} else {
+							/* desired thrust is in XY plane, set X downside to construct correct matrix,
+							 * but yaw component will not be used actually */
+							body_x.zero();
+							body_x(2) = 1.0f;
+						}
+
+						/* desired body_y axis */
+						body_y = body_z % body_x;
+
+						/* fill rotation matrix */
+						for (int i = 0; i < 3; i++) {
+							R(i, 0) = body_x(i);
+							R(i, 1) = body_y(i);
+							R(i, 2) = body_z(i);
+						}
+
+						/* copy rotation matrix to attitude setpoint topic */
+						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						_att_sp.R_valid = true;
+
+						/* calculate euler angles, for logging only, must not be used for control */
+						math::Vector<3> euler = R.to_euler();
+						_att_sp.roll_body = euler(0);
+						_att_sp.pitch_body = euler(1);
+						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+
+					} else if (!_control_mode.flag_control_manual_enabled) {
+						/* autonomous altitude control without position control (failsafe landing),
+						 * force level attitude, don't change yaw */
+						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+
+						/* copy rotation matrix to attitude setpoint topic */
+						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						_att_sp.R_valid = true;
+
+						_att_sp.roll_body = 0.0f;
+						_att_sp.pitch_body = 0.0f;
+					}
+
+					_att_sp.thrust = thrust_abs;
+
+					_att_sp.timestamp = hrt_absolute_time();
+
+					/* publish attitude setpoint */
+					if (_att_sp_pub > 0) {
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+					} else {
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+					}
+
+				} else {
+					reset_int_z = true;
+				}
+			}
+
+		} else {
+			/* position controller disabled, reset setpoints */
+			_reset_alt_sp = true;
+			_reset_pos_sp = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+
+		}
+
+		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+		reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
+	}
+
+	warnx("stopped");
+	mavlink_log_info(_mavlink_fd, "[mpc] stopped");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+MulticopterPositionControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("mc_pos_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       (main_t)&MulticopterPositionControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int mc_pos_control_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: mc_pos_control {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (pos_control::g_control != nullptr) {
+			errx(1, "already running");
+		}
+
+		pos_control::g_control = new MulticopterPositionControl;
+
+		if (pos_control::g_control == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != pos_control::g_control->start()) {
+			delete pos_control::g_control;
+			pos_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (pos_control::g_control == nullptr) {
+			errx(1, "not running");
+		}
+
+		delete pos_control::g_control;
+		pos_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (pos_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
new file mode 100644
index 000000000..05ab5769b
--- /dev/null
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author 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
+ * 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 mc_pos_control_params.c
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Minimum thrust
+ *
+ * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
+
+/**
+ * Maximum thrust
+ *
+ * Limit max allowed thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
+
+/**
+ * Proportional gain for vertical position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+
+/**
+ * Proportional gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+
+/**
+ * Integral gain for vertical velocity error
+ *
+ * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f);
+
+/**
+ * Differential gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+
+/**
+ * Maximum vertical velocity
+ *
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
+
+/**
+ * Vertical velocity feed forward
+ *
+ * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
+
+/**
+ * Proportional gain for horizontal position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
+
+/**
+ * Proportional gain for horizontal velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
+
+/**
+ * Integral gain for horizontal velocity error
+ *
+ * Non-zero value allows to resist wind.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
+
+/**
+ * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
+
+/**
+ * Maximum horizontal velocity
+ *
+ * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
+
+/**
+ * Horizontal velocity feed forward
+ *
+ * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
+
+/**
+ * Maximum tilt angle in air
+ *
+ * Limits maximum tilt in AUTO and POSCTRL modes during flight.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
+
+/**
+ * Maximum tilt during landing
+ *
+ * Limits maximum tilt angle on landing.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f);
+
+/**
+ * Landing descend rate
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
+
diff --git a/src/modules/mc_pos_control/module.mk b/src/modules/mc_pos_control/module.mk
new file mode 100644
index 000000000..0b566d7bd
--- /dev/null
+++ b/src/modules/mc_pos_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Build multicopter position controller
+#
+
+MODULE_COMMAND	= mc_pos_control
+
+SRCS		= mc_pos_control_main.cpp \
+			  mc_pos_control_params.c
diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/multirotor_att_control/module.mk
deleted file mode 100755
index 7569e1c7e..000000000
--- a/src/modules/multirotor_att_control/module.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 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.
-#
-############################################################################
-
-#
-# Build multirotor attitude controller
-#
-
-MODULE_COMMAND	= multirotor_att_control
-
-SRCS		= multirotor_att_control_main.c \
-		  multirotor_attitude_control.c \
-		  multirotor_rate_control.c
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
deleted file mode 100644
index 60a211817..000000000
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ /dev/null
@@ -1,465 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.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
- * 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 multirotor_att_control_main.c
- *
- * Implementation of multirotor attitude control main loop.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <getopt.h>
-#include <time.h>
-#include <math.h>
-#include <poll.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/param/param.h>
-
-#include "multirotor_attitude_control.h"
-#include "multirotor_rate_control.h"
-
-__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
-
-static bool thread_should_exit;
-static int mc_task;
-static bool motor_test_mode = false;
-static const float min_takeoff_throttle = 0.3f;
-static const float yaw_deadzone = 0.01f;
-
-static int
-mc_thread_main(int argc, char *argv[])
-{
-	/* declare and safely initialize all structs */
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_attitude_setpoint_s att_sp;
-	memset(&att_sp, 0, sizeof(att_sp));
-	struct offboard_control_setpoint_s offboard_sp;
-	memset(&offboard_sp, 0, sizeof(offboard_sp));
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct sensor_combined_s sensor;
-	memset(&sensor, 0, sizeof(sensor));
-	struct vehicle_rates_setpoint_s rates_sp;
-	memset(&rates_sp, 0, sizeof(rates_sp));
-	struct vehicle_status_s status;
-	memset(&status, 0, sizeof(status));
-	struct actuator_controls_s actuators;
-	memset(&actuators, 0, sizeof(actuators));
-
-	/* subscribe */
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* publish actuator controls */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
-		actuators.control[i] = 0.0f;
-	}
-
-	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
-	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
-	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-
-	/* register the perf counter */
-	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
-	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
-	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
-
-	warnx("starting");
-
-	/* store last control mode to detect mode switches */
-	bool control_yaw_position = true;
-	bool reset_yaw_sp = true;
-
-	struct pollfd fds[1] = {
-		{ .fd = vehicle_attitude_sub, .events = POLLIN },
-	};
-
-	while (!thread_should_exit) {
-
-		/* wait for a sensor update, check for exit condition every 500 ms */
-		int ret = poll(fds, 1, 500);
-
-		if (ret < 0) {
-			/* poll error, count it in perf */
-			perf_count(mc_err_perf);
-
-		} else if (ret > 0) {
-			/* only run controller if attitude changed */
-			perf_begin(mc_loop_perf);
-
-			/* attitude */
-			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-
-			bool updated;
-
-			/* parameters */
-			orb_check(parameter_update_sub, &updated);
-
-			if (updated) {
-				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
-				/* update parameters */
-			}
-
-			/* control mode */
-			orb_check(vehicle_control_mode_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode);
-			}
-
-			/* manual control setpoint */
-			orb_check(manual_control_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
-			}
-
-			/* attitude setpoint */
-			orb_check(vehicle_attitude_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
-			}
-
-			/* offboard control setpoint */
-			orb_check(offboard_control_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp);
-			}
-
-			/* vehicle status */
-			orb_check(vehicle_status_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
-			}
-
-			/* sensors */
-			orb_check(sensor_combined_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
-			}
-
-			/* set flag to safe value */
-			control_yaw_position = true;
-
-			/* reset yaw setpoint if not armed */
-			if (!control_mode.flag_armed) {
-				reset_yaw_sp = true;
-			}
-
-			/* define which input is the dominating control input */
-			if (control_mode.flag_control_offboard_enabled) {
-				/* offboard inputs */
-				if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
-					rates_sp.roll = offboard_sp.p1;
-					rates_sp.pitch = offboard_sp.p2;
-					rates_sp.yaw = offboard_sp.p3;
-					rates_sp.thrust = offboard_sp.p4;
-					rates_sp.timestamp = hrt_absolute_time();
-					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-
-				} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
-					att_sp.roll_body = offboard_sp.p1;
-					att_sp.pitch_body = offboard_sp.p2;
-					att_sp.yaw_body = offboard_sp.p3;
-					att_sp.thrust = offboard_sp.p4;
-					att_sp.timestamp = hrt_absolute_time();
-					/* publish the result to the vehicle actuators */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-				}
-
-				/* reset yaw setpoint after offboard control */
-				reset_yaw_sp = true;
-
-			} else if (control_mode.flag_control_manual_enabled) {
-				/* manual input */
-				if (control_mode.flag_control_attitude_enabled) {
-					/* control attitude, update attitude setpoint depending on mode */
-					if (att_sp.thrust < 0.1f) {
-						/* no thrust, don't try to control yaw */
-						rates_sp.yaw = 0.0f;
-						control_yaw_position = false;
-
-						if (status.condition_landed) {
-							/* reset yaw setpoint if on ground */
-							reset_yaw_sp = true;
-						}
-
-					} else {
-						/* only move yaw setpoint if manual input is != 0 */
-						if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
-							/* control yaw rate */
-							control_yaw_position = false;
-							rates_sp.yaw = manual.yaw;
-							reset_yaw_sp = true;	// has no effect on control, just for beautiful log
-
-						} else {
-							control_yaw_position = true;
-						}
-					}
-
-					if (!control_mode.flag_control_velocity_enabled) {
-						/* update attitude setpoint if not in position control mode */
-						att_sp.roll_body = manual.roll;
-						att_sp.pitch_body = manual.pitch;
-
-						if (!control_mode.flag_control_climb_rate_enabled) {
-							/* pass throttle directly if not in altitude control mode */
-							att_sp.thrust = manual.throttle;
-						}
-					}
-
-					/* reset yaw setpint to current position if needed */
-					if (reset_yaw_sp) {
-						att_sp.yaw_body = att.yaw;
-						reset_yaw_sp = false;
-					}
-
-					if (motor_test_mode) {
-						printf("testmode");
-						att_sp.roll_body = 0.0f;
-						att_sp.pitch_body = 0.0f;
-						att_sp.yaw_body = 0.0f;
-						att_sp.thrust = 0.1f;
-					}
-
-					att_sp.timestamp = hrt_absolute_time();
-
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
-				} else {
-					/* manual rate inputs (ACRO), from RC control or joystick */
-					if (control_mode.flag_control_rates_enabled) {
-						rates_sp.roll = manual.roll;
-						rates_sp.pitch = manual.pitch;
-						rates_sp.yaw = manual.yaw;
-						rates_sp.thrust = manual.throttle;
-						rates_sp.timestamp = hrt_absolute_time();
-					}
-
-					/* reset yaw setpoint after ACRO */
-					reset_yaw_sp = true;
-				}
-
-			} else {
-				if (!control_mode.flag_control_auto_enabled) {
-					/* no control, try to stay on place */
-					if (!control_mode.flag_control_velocity_enabled) {
-						/* no velocity control, reset attitude setpoint */
-						att_sp.roll_body = 0.0f;
-						att_sp.pitch_body = 0.0f;
-						att_sp.timestamp = hrt_absolute_time();
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-					}
-				}
-
-				/* reset yaw setpoint after non-manual control */
-				reset_yaw_sp = true;
-			}
-
-			/* check if we should we reset integrals */
-			bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
-
-			/* run attitude controller if needed */
-			if (control_mode.flag_control_attitude_enabled) {
-				multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
-				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-			}
-
-			/* measure in what intervals the controller runs */
-			perf_count(mc_interval_perf);
-
-			/* run rates controller if needed */
-			if (control_mode.flag_control_rates_enabled) {
-				/* get current rate setpoint */
-				bool rates_sp_updated = false;
-				orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated);
-
-				if (rates_sp_updated) {
-					orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp);
-				}
-
-				/* apply controller */
-				float rates[3];
-				rates[0] = att.rollspeed;
-				rates[1] = att.pitchspeed;
-				rates[2] = att.yawspeed;
-				multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
-
-			} else {
-				/* rates controller disabled, set actuators to zero for safety */
-				actuators.control[0] = 0.0f;
-				actuators.control[1] = 0.0f;
-				actuators.control[2] = 0.0f;
-				actuators.control[3] = 0.0f;
-			}
-
-			/* fill in manual control values */
-			actuators.control[4] = manual.flaps;
-			actuators.control[5] = manual.aux1;
-			actuators.control[6] = manual.aux2;
-			actuators.control[7] = manual.aux3;
-
-			actuators.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-
-			perf_end(mc_loop_perf);
-		}
-	}
-
-	warnx("stopping, disarming motors");
-
-	/* kill all outputs */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
-		actuators.control[i] = 0.0f;
-
-	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-
-	close(vehicle_attitude_sub);
-	close(vehicle_control_mode_sub);
-	close(manual_control_setpoint_sub);
-	close(actuator_pub);
-	close(att_sp_pub);
-
-	perf_print_counter(mc_loop_perf);
-	perf_free(mc_loop_perf);
-
-	fflush(stdout);
-	exit(0);
-}
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n");
-	fprintf(stderr, "    <mode> is 'rates' or 'attitude'\n");
-	fprintf(stderr, "    -t enables motor test mode with 10%% thrust\n");
-	exit(1);
-}
-
-int multirotor_att_control_main(int argc, char *argv[])
-{
-	int	ch;
-	unsigned int optioncount = 0;
-
-	while ((ch = getopt(argc, argv, "tm:")) != EOF) {
-		switch (ch) {
-		case 't':
-			motor_test_mode = true;
-			optioncount += 1;
-			break;
-
-		case ':':
-			usage("missing parameter");
-			break;
-
-		default:
-			fprintf(stderr, "option: -%c\n", ch);
-			usage("unrecognized option");
-			break;
-		}
-	}
-
-	argc -= optioncount;
-	//argv += optioncount;
-
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1 + optioncount], "start")) {
-
-		thread_should_exit = false;
-		mc_task = task_spawn_cmd("multirotor_att_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 15,
-					 2048,
-					 mc_thread_main,
-					 NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1 + optioncount], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
deleted file mode 100644
index 8245aa560..000000000
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ /dev/null
@@ -1,254 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           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
- * 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 multirotor_attitude_control.c
- *
- * Implementation of attitude controller for multirotors.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include "multirotor_attitude_control.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <float.h>
-#include <math.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/param/param.h>
-#include <drivers/drv_hrt.h>
-
-PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f);
-PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
-PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
-
-PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f);
-PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
-
-//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
-
-struct mc_att_control_params {
-	float yaw_p;
-	float yaw_i;
-	float yaw_d;
-	//float yaw_awu;
-	//float yaw_lim;
-
-	float att_p;
-	float att_i;
-	float att_d;
-	//float att_awu;
-	//float att_lim;
-
-	//float att_xoff;
-	//float att_yoff;
-};
-
-struct mc_att_control_param_handles {
-	param_t yaw_p;
-	param_t yaw_i;
-	param_t yaw_d;
-	//param_t yaw_awu;
-	//param_t yaw_lim;
-
-	param_t att_p;
-	param_t att_i;
-	param_t att_d;
-	//param_t att_awu;
-	//param_t att_lim;
-
-	//param_t att_xoff;
-	//param_t att_yoff;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-static int parameters_init(struct mc_att_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p);
-
-
-static int parameters_init(struct mc_att_control_param_handles *h)
-{
-	/* PID parameters */
-	h->yaw_p 	=	param_find("MC_YAWPOS_P");
-	h->yaw_i 	=	param_find("MC_YAWPOS_I");
-	h->yaw_d 	=	param_find("MC_YAWPOS_D");
-	//h->yaw_awu 	=	param_find("MC_YAWPOS_AWU");
-	//h->yaw_lim 	=	param_find("MC_YAWPOS_LIM");
-
-	h->att_p 	= 	param_find("MC_ATT_P");
-	h->att_i 	= 	param_find("MC_ATT_I");
-	h->att_d 	= 	param_find("MC_ATT_D");
-	//h->att_awu 	= 	param_find("MC_ATT_AWU");
-	//h->att_lim 	= 	param_find("MC_ATT_LIM");
-
-	//h->att_xoff 	= 	param_find("MC_ATT_XOFF");
-	//h->att_yoff 	= 	param_find("MC_ATT_YOFF");
-
-	return OK;
-}
-
-static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
-{
-	param_get(h->yaw_p, &(p->yaw_p));
-	param_get(h->yaw_i, &(p->yaw_i));
-	param_get(h->yaw_d, &(p->yaw_d));
-	//param_get(h->yaw_awu, &(p->yaw_awu));
-	//param_get(h->yaw_lim, &(p->yaw_lim));
-
-	param_get(h->att_p, &(p->att_p));
-	param_get(h->att_i, &(p->att_i));
-	param_get(h->att_d, &(p->att_d));
-	//param_get(h->att_awu, &(p->att_awu));
-	//param_get(h->att_lim, &(p->att_lim));
-
-	//param_get(h->att_xoff, &(p->att_xoff));
-	//param_get(h->att_yoff, &(p->att_yoff));
-
-	return OK;
-}
-
-void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral)
-{
-	static uint64_t last_run = 0;
-	static uint64_t last_input = 0;
-	float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
-
-	if (last_input != att_sp->timestamp) {
-		last_input = att_sp->timestamp;
-	}
-
-	static int motor_skip_counter = 0;
-
-	static PID_t pitch_controller;
-	static PID_t roll_controller;
-
-	static struct mc_att_control_params p;
-	static struct mc_att_control_param_handles h;
-
-	static bool initialized = false;
-
-	static float yaw_error;
-
-	/* initialize the pid controllers when the function is called for the first time */
-	if (initialized == false) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-
-		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
-		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
-
-		initialized = true;
-	}
-
-	/* load new parameters with lower rate */
-	if (motor_skip_counter % 500 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-
-		/* apply parameters */
-		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
-		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
-	}
-
-	/* reset integrals if needed */
-	if (reset_integral) {
-		pid_reset_integral(&pitch_controller);
-		pid_reset_integral(&roll_controller);
-		//TODO pid_reset_integral(&yaw_controller);
-	}
-
-	/* calculate current control outputs */
-
-	/* control pitch (forward) output */
-	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
-					att->pitch, att->pitchspeed, deltaT);
-
-	/* control roll (left/right) output */
-	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
-				       att->roll, att->rollspeed, deltaT);
-
-	if (control_yaw_position) {
-		/* control yaw rate */
-		// TODO use pid lib
-
-		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
-		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
-
-		yaw_error = att_sp->yaw_body - att->yaw;
-
-		if (yaw_error > M_PI_F) {
-			yaw_error -= M_TWOPI_F;
-
-		} else if (yaw_error < -M_PI_F) {
-			yaw_error += M_TWOPI_F;
-		}
-
-		rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
-	}
-
-	rates_sp->thrust = att_sp->thrust;
-    //need to update the timestamp now that we've touched rates_sp
-    rates_sp->timestamp = hrt_absolute_time();
-
-	motor_skip_counter++;
-}
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
deleted file mode 100644
index 86ac0e4ff..000000000
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ /dev/null
@@ -1,196 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           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
- * 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 multirotor_rate_control.c
- *
- * Implementation of rate controller for multirotors.
- *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#include "multirotor_rate_control.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <float.h>
-#include <math.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
-
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-
-struct mc_rate_control_params {
-
-	float yawrate_p;
-	float yawrate_d;
-	float yawrate_i;
-
-	float attrate_p;
-	float attrate_d;
-	float attrate_i;
-
-	float rate_lim;
-};
-
-struct mc_rate_control_param_handles {
-
-	param_t yawrate_p;
-	param_t yawrate_i;
-	param_t yawrate_d;
-
-	param_t attrate_p;
-	param_t attrate_i;
-	param_t attrate_d;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-static int parameters_init(struct mc_rate_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p);
-
-
-static int parameters_init(struct mc_rate_control_param_handles *h)
-{
-	/* PID parameters */
-	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
-	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
-	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
-
-	h->attrate_p 	= 	param_find("MC_ATTRATE_P");
-	h->attrate_i 	= 	param_find("MC_ATTRATE_I");
-	h->attrate_d 	= 	param_find("MC_ATTRATE_D");
-
-	return OK;
-}
-
-static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p)
-{
-	param_get(h->yawrate_p, &(p->yawrate_p));
-	param_get(h->yawrate_i, &(p->yawrate_i));
-	param_get(h->yawrate_d, &(p->yawrate_d));
-
-	param_get(h->attrate_p, &(p->attrate_p));
-	param_get(h->attrate_i, &(p->attrate_i));
-	param_get(h->attrate_d, &(p->attrate_d));
-
-	return OK;
-}
-
-void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
-{
-	static uint64_t last_run = 0;
-	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	static uint64_t last_input = 0;
-
-	if (last_input != rate_sp->timestamp) {
-		last_input = rate_sp->timestamp;
-	}
-
-	last_run = hrt_absolute_time();
-
-	static int motor_skip_counter = 0;
-
-	static PID_t pitch_rate_controller;
-	static PID_t roll_rate_controller;
-	static PID_t yaw_rate_controller;
-
-	static struct mc_rate_control_params p;
-	static struct mc_rate_control_param_handles h;
-
-	static bool initialized = false;
-
-	/* initialize the pid controllers when the function is called for the first time */
-	if (initialized == false) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-		initialized = true;
-
-		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-	}
-
-	/* load new parameters with lower rate */
-	if (motor_skip_counter % 2500 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
-		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
-		pid_set_parameters(&yaw_rate_controller,  p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
-	}
-
-	/* reset integrals if needed */
-	if (reset_integral) {
-		pid_reset_integral(&pitch_rate_controller);
-		pid_reset_integral(&roll_rate_controller);
-		pid_reset_integral(&yaw_rate_controller);
-	}
-
-	/* run pitch, roll and yaw controllers */
-	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
-	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
-	float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
-
-	actuators->control[0] = roll_control;
-	actuators->control[1] = pitch_control;
-	actuators->control[2] = yaw_control;
-	actuators->control[3] = rate_sp->thrust;
-
-	motor_skip_counter++;
-}
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
deleted file mode 100644
index 3d23d0c09..000000000
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ /dev/null
@@ -1,690 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: 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
- * 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 multirotor_pos_control.c
- *
- * Multirotor position controller
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <termios.h>
-#include <time.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_global_velocity_setpoint.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/pid/pid.h>
-#include <mavlink/mavlink_log.h>
-
-#include "multirotor_pos_control_params.h"
-#include "thrust_pid.h"
-
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of position controller.
- */
-static int multirotor_pos_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static float scale_control(float ctl, float end, float dz);
-
-static float norm(float x, float y);
-
-static void usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_spawn().
- */
-int multirotor_pos_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		warnx("start");
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("multirotor_pos_control",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_MAX - 60,
-					     4096,
-					     multirotor_pos_control_thread_main,
-					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		warnx("stop");
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("app is running");
-
-		} else {
-			warnx("app not started");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-static float scale_control(float ctl, float end, float dz)
-{
-	if (ctl > dz) {
-		return (ctl - dz) / (end - dz);
-
-	} else if (ctl < -dz) {
-		return (ctl + dz) / (end - dz);
-
-	} else {
-		return 0.0f;
-	}
-}
-
-static float norm(float x, float y)
-{
-	return sqrtf(x * x + y * y);
-}
-
-static int multirotor_pos_control_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	warnx("started");
-	static int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[mpc] started");
-
-	/* structures */
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_attitude_setpoint_s att_sp;
-	memset(&att_sp, 0, sizeof(att_sp));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct vehicle_local_position_s local_pos;
-	memset(&local_pos, 0, sizeof(local_pos));
-	struct vehicle_local_position_setpoint_s local_pos_sp;
-	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
-	struct vehicle_global_position_setpoint_s global_pos_sp;
-	memset(&global_pos_sp, 0, sizeof(global_pos_sp));
-	struct vehicle_global_velocity_setpoint_s global_vel_sp;
-	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
-
-	/* subscribe to attitude, motor setpoints and system state */
-	int param_sub = orb_subscribe(ORB_ID(parameter_update));
-	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-
-	/* publish setpoint */
-	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
-	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
-	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
-
-	bool reset_mission_sp = false;
-	bool global_pos_sp_valid = false;
-	bool reset_man_sp_z = true;
-	bool reset_man_sp_xy = true;
-	bool reset_int_z = true;
-	bool reset_int_z_manual = false;
-	bool reset_int_xy = true;
-	bool was_armed = false;
-	bool reset_auto_sp_xy = true;
-	bool reset_auto_sp_z = true;
-	bool reset_takeoff_sp = true;
-
-	hrt_abstime t_prev = 0;
-	const float alt_ctl_dz = 0.2f;
-	const float pos_ctl_dz = 0.05f;
-
-	float ref_alt = 0.0f;
-	hrt_abstime ref_alt_t = 0;
-	uint64_t local_ref_timestamp = 0;
-
-	PID_t xy_pos_pids[2];
-	PID_t xy_vel_pids[2];
-	PID_t z_pos_pid;
-	thrust_pid_t z_vel_pid;
-
-	thread_running = true;
-
-	struct multirotor_position_control_params params;
-	struct multirotor_position_control_param_handles params_h;
-	parameters_init(&params_h);
-	parameters_update(&params_h, &params);
-
-
-	for (int i = 0; i < 2; i++) {
-		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
-		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
-	}
-
-	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
-	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
-
-	while (!thread_should_exit) {
-
-		bool param_updated;
-		orb_check(param_sub, &param_updated);
-
-		if (param_updated) {
-			/* clear updated flag */
-			struct parameter_update_s ps;
-			orb_copy(ORB_ID(parameter_update), param_sub, &ps);
-			/* update params */
-			parameters_update(&params_h, &params);
-
-			for (int i = 0; i < 2; i++) {
-				pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
-				/* use integral_limit_out = tilt_max / 2 */
-				float i_limit;
-
-				if (params.xy_vel_i > 0.0f) {
-					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
-
-				} else {
-					i_limit = 0.0f;	// not used
-				}
-
-				pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
-			}
-
-			pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
-			thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
-		}
-
-		bool updated;
-
-		orb_check(control_mode_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-		}
-
-		orb_check(global_pos_sp_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
-			global_pos_sp_valid = true;
-			reset_mission_sp = true;
-		}
-
-		hrt_abstime t = hrt_absolute_time();
-		float dt;
-
-		if (t_prev != 0) {
-			dt = (t - t_prev) * 0.000001f;
-
-		} else {
-			dt = 0.0f;
-		}
-
-		if (control_mode.flag_armed && !was_armed) {
-			/* reset setpoints and integrals on arming */
-			reset_man_sp_z = true;
-			reset_man_sp_xy = true;
-			reset_auto_sp_z = true;
-			reset_auto_sp_xy = true;
-			reset_takeoff_sp = true;
-			reset_int_z = true;
-			reset_int_xy = true;
-		}
-
-		was_armed = control_mode.flag_armed;
-
-		t_prev = t;
-
-		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
-			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
-			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
-
-			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
-			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
-			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
-
-			if (control_mode.flag_control_manual_enabled) {
-				/* manual control */
-				/* check for reference point updates and correct setpoint */
-				if (local_pos.ref_timestamp != ref_alt_t) {
-					if (ref_alt_t != 0) {
-						/* home alt changed, don't follow large ground level changes in manual flight */
-						local_pos_sp.z += local_pos.ref_alt - ref_alt;
-					}
-
-					ref_alt_t = local_pos.ref_timestamp;
-					ref_alt = local_pos.ref_alt;
-					// TODO also correct XY setpoint
-				}
-
-				/* reset setpoints to current position if needed */
-				if (control_mode.flag_control_altitude_enabled) {
-					if (reset_man_sp_z) {
-						reset_man_sp_z = false;
-						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
-					}
-
-					/* move altitude setpoint with throttle stick */
-					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
-
-					if (z_sp_ctl != 0.0f) {
-						sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
-						local_pos_sp.z += sp_move_rate[2] * dt;
-
-						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
-							local_pos_sp.z = local_pos.z + z_sp_offs_max;
-
-						} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
-							local_pos_sp.z = local_pos.z - z_sp_offs_max;
-						}
-					}
-				}
-
-				if (control_mode.flag_control_position_enabled) {
-					if (reset_man_sp_xy) {
-						reset_man_sp_xy = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						pid_reset_integral(&xy_vel_pids[0]);
-						pid_reset_integral(&xy_vel_pids[1]);
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-					}
-
-					/* move position setpoint with roll/pitch stick */
-					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
-					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
-
-					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
-						/* calculate direction and increment of control in NED frame */
-						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
-						float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
-						sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
-						sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
-						local_pos_sp.x += sp_move_rate[0] * dt;
-						local_pos_sp.y += sp_move_rate[1] * dt;
-						/* limit maximum setpoint from position offset and preserve direction
-						 * fail safe, should not happen in normal operation */
-						float pos_vec_x = local_pos_sp.x - local_pos.x;
-						float pos_vec_y = local_pos_sp.y - local_pos.y;
-						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
-
-						if (pos_vec_norm > 1.0f) {
-							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
-							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
-						}
-					}
-				}
-
-				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
-				local_pos_sp.yaw = att_sp.yaw_body;
-
-				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
-				reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
-				reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
-				reset_takeoff_sp = true;
-
-				/* force reprojection of global setpoint after manual mode */
-				reset_mission_sp = true;
-
-			} else if (control_mode.flag_control_auto_enabled) {
-				/* AUTO mode, use global setpoint */
-				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-					if (reset_takeoff_sp) {
-						reset_takeoff_sp = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
-						att_sp.yaw_body = att.yaw;
-						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
-					}
-
-					reset_auto_sp_xy = false;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
-					// TODO
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
-					/* init local projection using local position ref */
-					if (local_pos.ref_timestamp != local_ref_timestamp) {
-						reset_mission_sp = true;
-						local_ref_timestamp = local_pos.ref_timestamp;
-						double lat_home = local_pos.ref_lat * 1e-7;
-						double lon_home = local_pos.ref_lon * 1e-7;
-						map_projection_init(lat_home, lon_home);
-						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
-					}
-
-					if (reset_mission_sp) {
-						reset_mission_sp = false;
-						/* update global setpoint projection */
-
-						if (global_pos_sp_valid) {
-							/* global position setpoint valid, use it */
-							double sp_lat = global_pos_sp.lat * 1e-7;
-							double sp_lon = global_pos_sp.lon * 1e-7;
-							/* project global setpoint to local setpoint */
-							map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
-
-							if (global_pos_sp.altitude_is_relative) {
-								local_pos_sp.z = -global_pos_sp.altitude;
-
-							} else {
-								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
-							}
-							/* update yaw setpoint only if value is valid */
-							if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
-								att_sp.yaw_body = global_pos_sp.yaw;
-							}
-
-							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
-
-						} else {
-							if (reset_auto_sp_xy) {
-								reset_auto_sp_xy = false;
-								/* local position setpoint is invalid,
-								 * use current position as setpoint for loiter */
-								local_pos_sp.x = local_pos.x;
-								local_pos_sp.y = local_pos.y;
-								local_pos_sp.yaw = att.yaw;
-							}
-
-							if (reset_auto_sp_z) {
-								reset_auto_sp_z = false;
-								local_pos_sp.z = local_pos.z;
-							}
-
-							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-						}
-					}
-
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-				}
-
-				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
-					reset_takeoff_sp = true;
-				}
-
-				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
-					reset_mission_sp = true;
-				}
-
-				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
-				local_pos_sp.yaw = att_sp.yaw_body;
-
-				/* reset setpoints after AUTO mode */
-				reset_man_sp_xy = true;
-				reset_man_sp_z = true;
-
-			} else {
-				/* no control (failsafe), loiter or stay on ground */
-				if (local_pos.landed) {
-					/* landed: move setpoint down */
-					/* in air: hold altitude */
-					if (local_pos_sp.z < 5.0f) {
-						/* set altitude setpoint to 5m under ground,
-						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
-						local_pos_sp.z = 5.0f;
-						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
-					}
-
-					reset_man_sp_z = true;
-
-				} else {
-					/* in air: hold altitude */
-					if (reset_man_sp_z) {
-						reset_man_sp_z = false;
-						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
-					}
-
-					reset_auto_sp_z = false;
-				}
-
-				if (control_mode.flag_control_position_enabled) {
-					if (reset_man_sp_xy) {
-						reset_man_sp_xy = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						local_pos_sp.yaw = att.yaw;
-						att_sp.yaw_body = att.yaw;
-						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-					}
-
-					reset_auto_sp_xy = false;
-				}
-			}
-
-			/* publish local position setpoint */
-			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
-
-			/* run position & altitude controllers, calculate velocity setpoint */
-			if (control_mode.flag_control_altitude_enabled) {
-				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
-
-			} else {
-				reset_man_sp_z = true;
-				global_vel_sp.vz = 0.0f;
-			}
-
-			if (control_mode.flag_control_position_enabled) {
-				/* calculate velocity set point in NED frame */
-				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
-				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
-
-				/* limit horizontal speed */
-				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
-
-				if (xy_vel_sp_norm > 1.0f) {
-					global_vel_sp.vx /= xy_vel_sp_norm;
-					global_vel_sp.vy /= xy_vel_sp_norm;
-				}
-
-			} else {
-				reset_man_sp_xy = true;
-				global_vel_sp.vx = 0.0f;
-				global_vel_sp.vy = 0.0f;
-			}
-
-			/* publish new velocity setpoint */
-			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
-			// TODO subscribe to velocity setpoint if altitude/position control disabled
-
-			if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
-				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
-				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
-
-				if (control_mode.flag_control_climb_rate_enabled) {
-					if (reset_int_z) {
-						reset_int_z = false;
-						float i = params.thr_min;
-
-						if (reset_int_z_manual) {
-							i = manual.throttle;
-
-							if (i < params.thr_min) {
-								i = params.thr_min;
-
-							} else if (i > params.thr_max) {
-								i = params.thr_max;
-							}
-						}
-
-						thrust_pid_set_integral(&z_vel_pid, -i);
-						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
-					}
-
-					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
-					att_sp.thrust = -thrust_sp[2];
-
-				} else {
-					/* reset thrust integral when altitude control enabled */
-					reset_int_z = true;
-				}
-
-				if (control_mode.flag_control_velocity_enabled) {
-					/* calculate thrust set point in NED frame */
-					if (reset_int_xy) {
-						reset_int_xy = false;
-						pid_reset_integral(&xy_vel_pids[0]);
-						pid_reset_integral(&xy_vel_pids[1]);
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
-					}
-
-					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
-					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
-
-					/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
-					/* limit horizontal part of thrust */
-					float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
-					/* assuming that vertical component of thrust is g,
-					 * horizontal component = g * tan(alpha) */
-					float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
-
-					if (tilt > params.tilt_max) {
-						tilt = params.tilt_max;
-					}
-
-					/* convert direction to body frame */
-					thrust_xy_dir -= att.yaw;
-					/* calculate roll and pitch */
-					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
-					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
-
-				} else {
-					reset_int_xy = true;
-				}
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				/* publish new attitude setpoint */
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-			}
-
-		} else {
-			/* position controller disabled, reset setpoints */
-			reset_man_sp_z = true;
-			reset_man_sp_xy = true;
-			reset_int_z = true;
-			reset_int_xy = true;
-			reset_mission_sp = true;
-			reset_auto_sp_xy = true;
-			reset_auto_sp_z = true;
-		}
-
-		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
-		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
-
-		/* run at approximately 50 Hz */
-		usleep(20000);
-	}
-
-	warnx("stopped");
-	mavlink_log_info(mavlink_fd, "[mpc] stopped");
-
-	thread_running = false;
-
-	fflush(stdout);
-	return 0;
-}
-
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
deleted file mode 100644
index b7041e4d5..000000000
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ /dev/null
@@ -1,112 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: 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
- * 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 multirotor_pos_control_params.c
- *
- * Parameters for multirotor_pos_control
- */
-
-#include "multirotor_pos_control_params.h"
-
-/* controller parameters */
-PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
-PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
-PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f);
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
-PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
-PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
-PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
-
-int parameters_init(struct multirotor_position_control_param_handles *h)
-{
-	h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
-	h->takeoff_gap = param_find("NAV_TAKEOFF_GAP");
-	h->thr_min 	=	param_find("MPC_THR_MIN");
-	h->thr_max 	=	param_find("MPC_THR_MAX");
-	h->z_p 	=	param_find("MPC_Z_P");
-	h->z_d 	=	param_find("MPC_Z_D");
-	h->z_vel_p 	=	param_find("MPC_Z_VEL_P");
-	h->z_vel_i 	=	param_find("MPC_Z_VEL_I");
-	h->z_vel_d 	=	param_find("MPC_Z_VEL_D");
-	h->z_vel_max 	=	param_find("MPC_Z_VEL_MAX");
-	h->xy_p 	=	param_find("MPC_XY_P");
-	h->xy_d 	=	param_find("MPC_XY_D");
-	h->xy_vel_p 	=	param_find("MPC_XY_VEL_P");
-	h->xy_vel_i 	=	param_find("MPC_XY_VEL_I");
-	h->xy_vel_d 	=	param_find("MPC_XY_VEL_D");
-	h->xy_vel_max 	=	param_find("MPC_XY_VEL_MAX");
-	h->tilt_max 	=	param_find("MPC_TILT_MAX");
-
-	h->rc_scale_pitch    =   param_find("RC_SCALE_PITCH");
-	h->rc_scale_roll    =   param_find("RC_SCALE_ROLL");
-	h->rc_scale_yaw      =   param_find("RC_SCALE_YAW");
-
-	return OK;
-}
-
-int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
-{
-	param_get(h->takeoff_alt, &(p->takeoff_alt));
-	param_get(h->takeoff_gap, &(p->takeoff_gap));
-	param_get(h->thr_min, &(p->thr_min));
-	param_get(h->thr_max, &(p->thr_max));
-	param_get(h->z_p, &(p->z_p));
-	param_get(h->z_d, &(p->z_d));
-	param_get(h->z_vel_p, &(p->z_vel_p));
-	param_get(h->z_vel_i, &(p->z_vel_i));
-	param_get(h->z_vel_d, &(p->z_vel_d));
-	param_get(h->z_vel_max, &(p->z_vel_max));
-	param_get(h->xy_p, &(p->xy_p));
-	param_get(h->xy_d, &(p->xy_d));
-	param_get(h->xy_vel_p, &(p->xy_vel_p));
-	param_get(h->xy_vel_i, &(p->xy_vel_i));
-	param_get(h->xy_vel_d, &(p->xy_vel_d));
-	param_get(h->xy_vel_max, &(p->xy_vel_max));
-	param_get(h->tilt_max, &(p->tilt_max));
-
-	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
-	param_get(h->rc_scale_roll, &(p->rc_scale_roll));
-	param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
-
-	return OK;
-}
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
deleted file mode 100644
index b985630ae..000000000
--- a/src/modules/multirotor_pos_control/thrust_pid.c
+++ /dev/null
@@ -1,189 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: 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
- * 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 thrust_pid.c
- *
- * Implementation of thrust control PID.
- *
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include "thrust_pid.h"
-#include <math.h>
-
-__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
-{
-	pid->kp = kp;
-	pid->ki = ki;
-	pid->kd = kd;
-	pid->limit_min = limit_min;
-	pid->limit_max = limit_max;
-	pid->mode = mode;
-	pid->dt_min = dt_min;
-	pid->last_output = 0.0f;
-	pid->sp = 0.0f;
-	pid->error_previous = 0.0f;
-	pid->integral = 0.0f;
-}
-
-__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
-{
-	int ret = 0;
-
-	if (isfinite(kp)) {
-		pid->kp = kp;
-
-	} else {
-		ret = 1;
-	}
-
-	if (isfinite(ki)) {
-		pid->ki = ki;
-
-	} else {
-		ret = 1;
-	}
-
-	if (isfinite(kd)) {
-		pid->kd = kd;
-
-	} else {
-		ret = 1;
-	}
-
-	if (isfinite(limit_min)) {
-		pid->limit_min = limit_min;
-
-	}  else {
-		ret = 1;
-	}
-
-	if (isfinite(limit_max)) {
-		pid->limit_max = limit_max;
-
-	}  else {
-		ret = 1;
-	}
-
-	return ret;
-}
-
-__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
-{
-	/* Alternative integral component calculation
-	 *
-	 * start:
-	 * error = setpoint - current_value
-	 * integral = integral + (Ki * error * dt)
-	 * derivative = (error - previous_error) / dt
-	 * previous_error = error
-	 * output = (Kp * error) + integral + (Kd * derivative)
-	 * wait(dt)
-	 * goto start
-	 */
-
-	if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
-		return pid->last_output;
-	}
-
-	float i, d;
-	pid->sp = sp;
-
-	// Calculated current error value
-	float error = pid->sp - val;
-
-	// Calculate or measured current error derivative
-	if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = error;
-
-	} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
-		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = -val;
-
-	} else {
-		d = 0.0f;
-	}
-
-	if (!isfinite(d)) {
-		d = 0.0f;
-	}
-
-	/* calculate the error integral */
-	i = pid->integral + (pid->ki * error * dt);
-
-	/* attitude-thrust compensation
-	 * r22 is (2, 2) componet of rotation matrix for current attitude */
-	float att_comp;
-
-	if (r22 > 0.8f)
-		att_comp = 1.0f / r22;
-	else if (r22 > 0.0f)
-		att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
-	else
-		att_comp = 1.0f;
-
-	/* calculate output */
-	float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
-
-	/* check for saturation */
-	if (output < pid->limit_min || output > pid->limit_max) {
-		/* saturated, recalculate output with old integral */
-		output = (error * pid->kp) + pid->integral + (d * pid->kd);
-
-	} else {
-		if (isfinite(i)) {
-			pid->integral = i;
-		}
-	}
-
-	if (isfinite(output)) {
-		if (output > pid->limit_max) {
-			output = pid->limit_max;
-
-		} else if (output < pid->limit_min) {
-			output = pid->limit_min;
-		}
-
-		pid->last_output = output;
-	}
-
-	return pid->last_output;
-}
-
-__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
-{
-	pid->integral = i;
-}
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
new file mode 100644
index 000000000..266215308
--- /dev/null
+++ b/src/modules/navigator/geofence.cpp
@@ -0,0 +1,298 @@
+/****************************************************************************
+ *
+ *   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 geofence.cpp
+ * Provides functions for handling the geofence
+ *
+ * @author Jean Cyr <jean.m.cyr@gmail.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include "geofence.h"
+
+#include <uORB/topics/vehicle_global_position.h>
+#include <string.h>
+#include <dataman/dataman.h>
+#include <systemlib/err.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <nuttx/config.h>
+#include <unistd.h>
+
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+Geofence::Geofence() :
+		SuperBlock(NULL, "GF"),
+		_fence_pub(-1),
+		_altitude_min(0),
+		_altitude_max(0),
+		_verticesCount(0),
+		param_geofence_on(this, "ON")
+{
+	/* Load initial params */
+	updateParams();
+}
+
+Geofence::~Geofence()
+{
+
+}
+
+
+bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
+{
+	double lat = vehicle->lat / 1e7d;
+	double lon = vehicle->lon / 1e7d;
+	//float	alt = vehicle->alt;
+
+	return inside(lat, lon, vehicle->alt);
+}
+
+bool Geofence::inside(double lat, double lon, float altitude)
+{
+	/* Return true if geofence is disabled */
+	if (param_geofence_on.get() != 1)
+		return true;
+
+	if (valid()) {
+
+		if (!isEmpty()) {
+			/* Vertical check */
+			if (altitude > _altitude_max || altitude < _altitude_min)
+				return false;
+
+			/*Horizontal check */
+			/* Adaptation of algorithm originally presented as
+			 * PNPOLY - Point Inclusion in Polygon Test
+			 * W. Randolph Franklin (WRF) */
+
+			bool c = false;
+
+			struct fence_vertex_s temp_vertex_i;
+			struct fence_vertex_s temp_vertex_j;
+
+			/* Red until fence is finished */
+			for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
+				if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+					break;
+				}
+				if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+					break;
+				}
+
+				// skip vertex 0 (return point)
+				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;
+				}
+
+			}
+
+			return c;
+		} else {
+			/* Empty fence --> accept all points */
+			return true;
+		}
+
+	} else {
+		/* Invalid fence --> accept all points */
+		return true;
+	}
+}
+
+bool
+Geofence::valid()
+{
+	// NULL fence is valid
+	if (isEmpty())
+		return true;
+
+	// Otherwise
+	if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
+		warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
+		return false;
+	}
+
+	return true;
+}
+
+void
+Geofence::addPoint(int argc, char *argv[])
+{
+	int ix, last;
+	double lon, lat;
+	struct fence_vertex_s vertex;
+	char *end;
+
+	if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) {
+		dm_clear(DM_KEY_FENCE_POINTS);
+		publishFence(0);
+		return;
+	}
+
+	if (argc < 3)
+		errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
+
+	ix = atoi(argv[0]);
+	if (ix >= DM_KEY_FENCE_POINTS_MAX)
+		errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
+
+	lat = strtod(argv[1], &end);
+	lon = strtod(argv[2], &end);
+
+	last = 0;
+	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
+		last = 1;
+
+	vertex.lat = (float)lat;
+	vertex.lon = (float)lon;
+
+	if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
+		if (last)
+			publishFence((unsigned)ix + 1);
+		return;
+	}
+
+	errx(1, "can't store fence point");
+}
+
+void
+Geofence::publishFence(unsigned vertices)
+{
+	if (_fence_pub == -1)
+		_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
+	else
+		orb_publish(ORB_ID(fence), _fence_pub, &vertices);
+}
+
+int
+Geofence::loadFromFile(const char *filename)
+{
+	FILE		*fp;
+	char		line[120];
+	int			pointCounter = 0;
+	bool		gotVertical = false;
+	const char commentChar = '#';
+
+	/* Make sure no data is left in the datamanager */
+	clearDm();
+
+	/* open the mixer definition file */
+	fp = fopen(GEOFENCE_FILENAME, "r");
+	if (fp == NULL) {
+		return ERROR;
+	}
+
+	/* create geofence points from valid lines and store in DM */
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* Trim leading whitespace */
+		size_t textStart = 0;
+		while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
+
+		/* if the line starts with #, skip */
+		if (line[textStart] == commentChar)
+			continue;
+
+		if (gotVertical) {
+			/* Parse the line as a geofence point */
+			struct fence_vertex_s vertex;
+
+			/* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */
+			if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') {
+				/* Handle degree minute second format */
+				float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
+
+				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
+					return ERROR;
+
+//				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
+
+				vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f;
+				vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f;
+
+			} else {
+				/* Handle decimal degree format */
+
+				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
+					return ERROR;
+			}
+
+			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
+								return ERROR;
+
+			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter,  (double)vertex.lat, (double)vertex.lon);
+
+			pointCounter++;
+		} else {
+			/* Parse the line as the vertical limits */
+			if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2)
+				return ERROR;
+
+
+			warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max);
+			gotVertical = true;
+		}
+
+
+	}
+
+	fclose(fp);
+
+	/* Check if import was successful */
+	if(gotVertical && pointCounter > 0)
+	{
+		_verticesCount = pointCounter;
+		warnx("Geofence: imported successfully");
+	} else {
+		warnx("Geofence: import error");
+	}
+
+	return ERROR;
+}
+
+int Geofence::clearDm()
+{
+	dm_clear(DM_KEY_FENCE_POINTS);
+	return OK;
+}
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
new file mode 100644
index 000000000..2eb126ab5
--- /dev/null
+++ b/src/modules/navigator/geofence.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   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 geofence.h
+ * Provides functions for handling the geofence
+ *
+ * @author Jean Cyr <jean.m.cyr@gmail.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef GEOFENCE_H_
+#define GEOFENCE_H_
+
+#include <uORB/topics/fence.h>
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
+
+class Geofence : public control::SuperBlock
+{
+private:
+	orb_advert_t	_fence_pub;			/**< publish fence topic */
+
+	float			_altitude_min;
+	float			_altitude_max;
+
+	unsigned 			_verticesCount;
+
+	/* Params */
+	control::BlockParamInt param_geofence_on;
+public:
+	Geofence();
+	~Geofence();
+
+	/**
+	 * Return whether craft is inside geofence.
+	 *
+	 * Calculate whether point is inside arbitrary polygon
+	 * @param craft pointer craft coordinates
+	 * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
+	 * @return true: craft is inside fence, false:craft is outside fence
+	 */
+	bool inside(const struct vehicle_global_position_s *craft);
+	bool inside(double lat, double lon, float altitude);
+
+	int clearDm();
+
+	bool valid();
+
+	/**
+	 * Specify fence vertex position.
+	 */
+	void addPoint(int argc, char *argv[]);
+
+	void publishFence(unsigned vertices);
+
+	int loadFromFile(const char *filename);
+
+	bool isEmpty() {return _verticesCount == 0;}
+};
+
+
+#endif /* GEOFENCE_H_ */
diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/modules/navigator/geofence_params.c
index 7ea6496bb..653b1ad84 100644
--- a/src/lib/mathlib/math/generic/Vector.cpp
+++ b/src/modules/navigator/geofence_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -17,7 +17,7 @@
  *    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
+ * 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,
@@ -32,9 +32,29 @@
  ****************************************************************************/
 
 /**
- * @file Vector.cpp
+ * @file geofence_params.c
  *
- * math vector
+ * Parameters for geofence
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Geofence parameters, accessible via MAVLink
  */
 
-#include "Vector.hpp"
+/**
+ * Enable geofence.
+ *
+ * Set to 1 to enable geofence.
+ * Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
+ *
+ * @min 0
+ * @max 1
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_ON, 1);
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/navigator/loiter.cpp
index 5e169c1ba..f827e70c9 100644
--- a/src/modules/multirotor_pos_control/thrust_pid.h
+++ b/src/modules/navigator/loiter.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   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
@@ -31,46 +30,64 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 /**
- * @file thrust_pid.h
+ * @file loiter.cpp
  *
- * Definition of thrust control PID interface.
+ * Helper class to loiter
  *
+ * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#ifndef THRUST_PID_H_
-#define THRUST_PID_H_
+#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"
+#include "navigator.h"
+
+Loiter::Loiter(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name)
+{
+	/* load initial params */
+	updateParams();
+}
 
-#include <stdint.h>
+Loiter::~Loiter()
+{
+}
 
-__BEGIN_DECLS
+void
+Loiter::on_inactive()
+{
+}
 
-/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
-#define THRUST_PID_MODE_DERIVATIV_CALC	0
-/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
-#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP	1
+void
+Loiter::on_activation()
+{
+	/* set current mission item to loiter */
+	set_loiter_item(&_mission_item);
 
-typedef struct {
-	float kp;
-	float ki;
-	float kd;
-	float sp;
-	float integral;
-	float error_previous;
-	float last_output;
-	float limit_min;
-	float limit_max;
-	float dt_min;
-	uint8_t mode;
-} thrust_pid_t;
+	/* convert mission item to current setpoint */
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	pos_sp_triplet->previous.valid = false;
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
 
-__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
-__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
-__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
-__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
+	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
 
-__END_DECLS
+	_navigator->set_position_setpoint_triplet_updated();
+}
 
-#endif /* THRUST_PID_H_ */
+void
+Loiter::on_active()
+{
+}
diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/navigator/loiter.h
index 744ed7d94..37ab57a07 100644
--- a/src/modules/mavlink/mavlink_hil.h
+++ b/src/modules/navigator/loiter.h
@@ -1,7 +1,6 @@
-/****************************************************************************
+/***************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -31,22 +30,35 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 /**
- * @file mavlink_hil.h
- * Hardware-in-the-loop simulation support.
+ * @file loiter.h
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#pragma once
+#ifndef NAVIGATOR_LOITER_H
+#define NAVIGATOR_LOITER_H
 
-extern bool mavlink_hil_enabled;
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
 
-/**
- * Enable / disable Hardware in the Loop simulation mode.
- *
- * @param hil_enabled	The new HIL enable/disable state.
- * @return		OK if the HIL state changed, ERROR if the
- *			requested change could not be made or was
- *			redundant.
- */
-extern int set_hil_on_off(bool hil_enabled);
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Loiter : public MissionBlock
+{
+public:
+	Loiter(Navigator *navigator, const char *name);
+
+	~Loiter();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+};
+
+#endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
new file mode 100644
index 000000000..ba766cd10
--- /dev/null
+++ b/src/modules/navigator/mission.cpp
@@ -0,0 +1,618 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ *
+ * Helper class to access missions
+ *
+ * @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 <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+#include "mission.h"
+#include "navigator.h"
+
+Mission::Mission(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_param_takeoff_alt(this, "TAKEOFF_ALT"),
+	_param_dist_1wp(this, "DIST_1WP"),
+	_onboard_mission({0}),
+	_offboard_mission({0}),
+	_current_onboard_mission_index(-1),
+	_current_offboard_mission_index(-1),
+	_need_takeoff(true),
+	_takeoff(false),
+	_mission_result_pub(-1),
+	_mission_result({0}),
+	_mission_type(MISSION_TYPE_NONE),
+	_inited(false),
+	_dist_1wp_ok(false)
+{
+	/* load initial params */
+	updateParams();
+}
+
+Mission::~Mission()
+{
+}
+
+void
+Mission::on_inactive()
+{
+	if (_inited) {
+		/* check if missions have changed so that feedback to ground station is given */
+		bool onboard_updated = false;
+		orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+		if (onboard_updated) {
+			update_onboard_mission();
+		}
+
+		bool offboard_updated = false;
+		orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+		if (offboard_updated) {
+			update_offboard_mission();
+		}
+
+	} else {
+		/* read mission topics on initialization */
+		_inited = true;
+
+		update_onboard_mission();
+		update_offboard_mission();
+	}
+
+	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
+		_need_takeoff = true;
+	}
+}
+
+void
+Mission::on_activation()
+{
+	set_mission_items();
+}
+
+void
+Mission::on_active()
+{
+	/* check if anything has changed */
+	bool onboard_updated = false;
+	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+	if (onboard_updated) {
+		update_onboard_mission();
+	}
+
+	bool offboard_updated = false;
+	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) {
+		set_mission_items();
+	}
+
+	/* lets check if we reached the current mission item */
+	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		advance_mission();
+		set_mission_items();
+
+	} else {
+		/* if waypoint position reached allow loiter on the setpoint */
+		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
+			_navigator->set_can_loiter_at_sp(true);
+		}
+	}
+}
+
+void
+Mission::update_onboard_mission()
+{
+	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_seq >=0
+		&& _onboard_mission.current_seq < (int)_onboard_mission.count) {
+			_current_onboard_mission_index = _onboard_mission.current_seq;
+		} 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_seq = 0;
+		_current_onboard_mission_index = 0;
+	}
+}
+
+void
+Mission::update_offboard_mission()
+{
+	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
+		warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
+		/* determine current index */
+		if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
+			_current_offboard_mission_index = _offboard_mission.current_seq;
+		} else {
+			/* if less items available, reset to first item */
+			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 */
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+
+		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+				_navigator->get_home_position()->alt);
+
+	} else {
+		warnx("offboard mission update failed");
+		_offboard_mission.count = 0;
+		_offboard_mission.current_seq = 0;
+		_current_offboard_mission_index = 0;
+	}
+
+	report_current_offboard_mission_item();
+}
+
+
+void
+Mission::advance_mission()
+{
+	if (_takeoff) {
+		_takeoff = false;
+
+	} else {
+		switch (_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;
+		}
+	}
+}
+
+bool
+Mission::check_dist_1wp()
+{
+	if (_dist_1wp_ok) {
+		/* always return true after at least one successful check */
+		return true;
+	}
+
+	/* check if first waypoint is not too far from home */
+	if (_param_dist_1wp.get() > 0.0f) {
+		if (_navigator->get_vstatus()->condition_home_position_valid) {
+			struct mission_item_s mission_item;
+
+			/* find first waypoint (with lat/lon) item in datamanager */
+			for (unsigned i = 0; i < _offboard_mission.count; i++) {
+				if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
+						&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
+
+					/* check only items with valid lat/lon */
+					if (	mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+							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 ||
+							mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+							mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
+
+						/* check distance from home to item */
+						float dist_to_1wp = get_distance_to_next_waypoint(
+								mission_item.lat, mission_item.lon,
+								_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
+
+						if (dist_to_1wp < _param_dist_1wp.get()) {
+							_dist_1wp_ok = true;
+							return true;
+
+						} else {
+							/* item is too far from home */
+							mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
+							return false;
+						}
+					}
+
+				} else {
+					/* error reading, mission is invalid */
+					mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
+					return false;
+				}
+			}
+
+			/* no waypoints found in mission, then we will not fly far away */
+			_dist_1wp_ok = true;
+			return true;
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
+			return false;
+		}
+
+	} else {
+		return true;
+	}
+}
+
+void
+Mission::set_mission_items()
+{
+	/* make sure param is up to date */
+	updateParams();
+
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* set previous position setpoint to current */
+	set_previous_pos_setpoint();
+
+	/* try setting onboard mission item */
+	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_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;
+
+	/* try setting offboard mission item */
+	} else if (check_dist_1wp() && read_mission_item(false, true, &_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;
+
+	} else {
+		/* no mission available, switch to loiter */
+		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;
+
+		/* set loiter mission item */
+		set_loiter_item(&_mission_item);
+
+		/* update position setpoint triplet  */
+		pos_sp_triplet->previous.valid = false;
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		pos_sp_triplet->next.valid = false;
+
+		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+
+		reset_mission_item_reached();
+		report_mission_finished();
+
+		_navigator->set_position_setpoint_triplet_updated();
+		return;
+	}
+
+	/* do takeoff on first waypoint for rotary wing vehicles */
+	if (_navigator->get_vstatus()->is_rotary_wing) {
+		/* force takeoff if landed (additional protection) */
+		if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
+			_need_takeoff = true;
+		}
+
+		/* new current mission item set, check if we need takeoff */
+		if (_need_takeoff && (
+				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+				_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 ||
+				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
+			_takeoff = true;
+			_need_takeoff = false;
+		}
+	}
+
+	if (_takeoff) {
+		/* do takeoff before going to setpoint */
+		/* set mission item as next position setpoint */
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
+
+		/* calculate takeoff altitude */
+		float takeoff_alt = _mission_item.altitude;
+		if (_mission_item.altitude_is_relative) {
+			takeoff_alt += _navigator->get_home_position()->alt;
+		}
+
+		/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
+		if (_navigator->get_vstatus()->condition_landed) {
+			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
+
+		} else {
+			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
+		}
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude = takeoff_alt;
+		_mission_item.altitude_is_relative = false;
+
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+
+	} else {
+		/* set current position setpoint from mission item */
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+
+		/* require takeoff after landing or idle */
+		if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
+			_need_takeoff = true;
+		}
+
+		_navigator->set_can_loiter_at_sp(false);
+		reset_mission_item_reached();
+
+		if (_mission_type == MISSION_TYPE_OFFBOARD) {
+			report_current_offboard_mission_item();
+		}
+		// TODO: report onboard mission item somehow
+
+		/* try to read next mission item */
+		struct mission_item_s mission_item_next;
+
+		if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
+			/* got next mission item, update setpoint triplet */
+			mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
+
+		} else {
+			/* next mission item is not available */
+			pos_sp_triplet->next.valid = false;
+		}
+	}
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+bool
+Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
+{
+	/* select onboard/offboard mission */
+	int *mission_index_ptr;
+	struct mission_s *mission;
+	dm_item_t dm_item;
+	int mission_index_next;
+
+	if (onboard) {
+		/* onboard mission */
+		mission_index_next = _current_onboard_mission_index + 1;
+		mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
+
+		mission = &_onboard_mission;
+
+		dm_item = DM_KEY_WAYPOINTS_ONBOARD;
+
+	} else {
+		/* offboard mission */
+		mission_index_next = _current_offboard_mission_index + 1;
+		mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
+
+		mission = &_offboard_mission;
+
+		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+	}
+
+	if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+		/* mission item index out of bounds */
+		return false;
+	}
+
+	/* 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 to temp storage first to not overwrite current mission item if data damaged */
+		struct mission_item_s mission_item_tmp;
+
+		/* read mission item from datamanager */
+		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, 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 (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {
+
+			/* do DO_JUMP as many times as requested */
+			if (mission_item_tmp.do_jump_current_count < mission_item_tmp.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) {
+					(mission_item_tmp.do_jump_current_count)++;
+					/* save repeat count */
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, 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_ptr = mission_item_tmp.do_jump_mission_index;
+
+			} else {
+				mavlink_log_info(_navigator->get_mavlink_fd(),
+						 "#audio: DO JUMP repetitions completed");
+				/* no more DO_JUMPS, therefore just try to continue with next mission item */
+				(*mission_index_ptr)++;
+			}
+
+		} else {
+			/* if it's not a DO_JUMP, then we were successful */
+			memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
+			return true;
+		}
+	}
+
+	/* we have given up, we don't want to cycle forever */
+	mavlink_log_critical(_navigator->get_mavlink_fd(),
+			     "#audio: ERROR DO JUMP is cycling, giving up");
+	return false;
+}
+
+void
+Mission::save_offboard_mission_state()
+{
+	mission_s mission_state;
+
+	/* lock MISSION_STATE item */
+	dm_lock(DM_KEY_MISSION_STATE);
+
+	/* read current state */
+	int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+	if (read_res == sizeof(mission_s)) {
+		/* data read successfully, check dataman ID and items count */
+		if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) {
+			/* navigator may modify only sequence, write modified state only if it changed */
+			if (mission_state.current_seq != _current_offboard_mission_index) {
+				if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+					warnx("ERROR: can't save mission state");
+					mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
+				}
+			}
+		}
+
+	} else {
+		/* invalid data, this must not happen and indicates error in offboard_mission publisher */
+		mission_state.dataman_id = _offboard_mission.dataman_id;
+		mission_state.count = _offboard_mission.count;
+		mission_state.current_seq = _current_offboard_mission_index;
+
+		warnx("ERROR: invalid mission state");
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state");
+
+		/* write modified state only if changed */
+		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+			warnx("ERROR: can't save mission state");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
+		}
+	}
+
+	/* unlock MISSION_STATE item */
+	dm_unlock(DM_KEY_MISSION_STATE);
+}
+
+void
+Mission::report_mission_item_reached()
+{
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
+		_mission_result.reached = true;
+		_mission_result.seq_reached = _current_offboard_mission_index;
+	}
+	publish_mission_result();
+}
+
+void
+Mission::report_current_offboard_mission_item()
+{
+	warnx("current offboard mission index: %d", _current_offboard_mission_index);
+	_mission_result.seq_current = _current_offboard_mission_index;
+	publish_mission_result();
+
+	save_offboard_mission_state();
+}
+
+void
+Mission::report_mission_finished()
+{
+	_mission_result.finished = true;
+	publish_mission_result();
+}
+
+void
+Mission::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+	} else {
+		/* advertise and publish */
+		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+	}
+	/* reset reached bool */
+	_mission_result.reached = false;
+	_mission_result.finished = false;
+}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
new file mode 100644
index 000000000..4da6a1155
--- /dev/null
+++ b/src/modules/navigator/mission.h
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   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 mission.h
+ *
+ * Navigator mode to access missions
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_MISSION_H
+#define NAVIGATOR_MISSION_H
+
+#include <drivers/drv_hrt.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <dataman/dataman.h>
+
+#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 MissionBlock
+{
+public:
+	Mission(Navigator *navigator, const char *name);
+
+	virtual ~Mission();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/**
+	 * Update onboard mission topic
+	 */
+	void update_onboard_mission();
+
+	/**
+	 * Update offboard mission topic
+	 */
+	void update_offboard_mission();
+
+	/**
+	 * Move on to next mission item or switch to loiter
+	 */
+	void advance_mission();
+
+	/**
+	 * Check distance to first waypoint (with lat/lon)
+	 * @return true only if it's not too far from home (< MIS_DIST_1WP)
+	 */
+	bool check_dist_1wp();
+
+	/**
+	 * Set new mission items
+	 */
+	void set_mission_items();
+
+	/**
+	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
+	 * @return true if successful
+	 */
+	bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item);
+
+	/**
+	 * Save current offboard mission state to dataman
+	 */
+	void save_offboard_mission_state();
+
+	/**
+	 * Report that a mission item has been reached
+	 */
+	void report_mission_item_reached();
+
+	/**
+	 * Rport the current mission item
+	 */
+	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
+	 */
+	void publish_mission_result();
+
+	control::BlockParamInt _param_onboard_enabled;
+	control::BlockParamFloat _param_takeoff_alt;
+	control::BlockParamFloat _param_dist_1wp;
+
+	struct mission_s _onboard_mission;
+	struct mission_s _offboard_mission;
+
+	int _current_onboard_mission_index;
+	int _current_offboard_mission_index;
+	bool _need_takeoff;
+	bool _takeoff;
+
+	orb_advert_t _mission_result_pub;
+	struct mission_result_s _mission_result;
+
+	enum {
+		MISSION_TYPE_NONE,
+		MISSION_TYPE_ONBOARD,
+		MISSION_TYPE_OFFBOARD
+	} _mission_type;
+
+	bool _inited;
+	bool _dist_1wp_ok;
+
+	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 000000000..4adf77dce
--- /dev/null
+++ b/src/modules/navigator/mission_block.cpp
@@ -0,0 +1,251 @@
+/****************************************************************************
+ *
+ *   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 <math.h>
+#include <float.h>
+
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <mavlink/mavlink_log.h>
+
+#include <uORB/uORB.h>
+
+#include "navigator.h"
+#include "mission_block.h"
+
+
+MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	_mission_item({0}),
+	_waypoint_position_reached(false),
+	_waypoint_yaw_reached(false),
+	_time_first_inside_orbit(0)
+{
+}
+
+MissionBlock::~MissionBlock()
+{
+}
+
+bool
+MissionBlock::is_mission_item_reached()
+{
+	if (_mission_item.nav_cmd == NAV_CMD_IDLE) {
+		return false;
+	}
+
+	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+		return _navigator->get_vstatus()->condition_landed;
+	}
+
+	/* TODO: count turns */
+	if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
+		return false;
+	}
+
+	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 && _navigator->get_vstatus()->is_rotary_wing) {
+			/* require only altitude for takeoff for multicopter */
+			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->get_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;
+			}
+		}
+	}
+
+	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
+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->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;
+
+	switch (item->nav_cmd) {
+	case NAV_CMD_IDLE:
+		sp->type = SETPOINT_TYPE_IDLE;
+		break;
+
+	case NAV_CMD_TAKEOFF:
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+		break;
+
+	case NAV_CMD_LAND:
+		sp->type = SETPOINT_TYPE_LAND;
+		break;
+
+	case NAV_CMD_LOITER_TIME_LIMIT:
+	case NAV_CMD_LOITER_TURN_COUNT:
+	case NAV_CMD_LOITER_UNLIMITED:
+		sp->type = SETPOINT_TYPE_LOITER;
+		break;
+
+	default:
+		sp->type = SETPOINT_TYPE_POSITION;
+		break;
+	}
+}
+
+void
+MissionBlock::set_previous_pos_setpoint()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+    if (pos_sp_triplet->current.valid) {
+        memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
+    }
+}
+
+void
+MissionBlock::set_loiter_item(struct mission_item_s *item)
+{
+	if (_navigator->get_vstatus()->condition_landed) {
+		/* landed, don't takeoff, but switch to IDLE mode */
+		item->nav_cmd = NAV_CMD_IDLE;
+
+	} else {
+		item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+		if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
+			/* use current position setpoint */
+			item->lat = pos_sp_triplet->current.lat;
+			item->lon = pos_sp_triplet->current.lon;
+			item->altitude = pos_sp_triplet->current.alt;
+
+		} else {
+			/* use current position */
+			item->lat = _navigator->get_global_position()->lat;
+			item->lon = _navigator->get_global_position()->lon;
+			item->altitude = _navigator->get_global_position()->alt;
+		}
+
+		item->altitude_is_relative = false;
+		item->yaw = NAN;
+		item->loiter_radius = _navigator->get_loiter_radius();
+		item->loiter_direction = 1;
+		item->acceptance_radius = _navigator->get_acceptance_radius();
+		item->time_inside = 0.0f;
+		item->pitch_min = 0.0f;
+		item->autocontinue = false;
+		item->origin = ORIGIN_ONBOARD;
+	}
+}
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
new file mode 100644
index 000000000..adf50bc39
--- /dev/null
+++ b/src/modules/navigator/mission_block.h
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   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>
+
+#include "navigator_mode.h"
+
+class Navigator;
+
+class MissionBlock : public NavigatorMode
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MissionBlock(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~MissionBlock();
+
+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
+	 *
+	 * @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 previous position setpoint to current setpoint
+     */
+	void set_previous_pos_setpoint();
+
+	/**
+	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
+	 */
+	void set_loiter_item(struct mission_item_s *item);
+
+	mission_item_s _mission_item;
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
+};
+
+#endif
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
new file mode 100644
index 000000000..dd7f4c801
--- /dev/null
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -0,0 +1,234 @@
+/****************************************************************************
+ *
+ *   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 mission_feasibility_checker.cpp
+ * Provides checks if mission is feasible given the navigation capabilities
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ */
+
+#include "mission_feasibility_checker.h"
+
+#include <geo/geo.h>
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+#include <fw_pos_control_l1/landingslope.h>
+#include <systemlib/err.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <uORB/topics/fence.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
+{
+	_nav_caps = {0};
+}
+
+
+bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+{
+	/* Init if not done yet */
+	init();
+
+	/* 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);
+	}
+
+
+	if (isRotarywing)
+		return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
+	else
+		return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
+}
+
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+{
+
+	return  (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+}
+
+bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+{
+	/* Update fixed wing navigation capabilites */
+	updateNavigationCapabilities();
+//	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
+
+	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+}
+
+bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+{
+	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
+	if (geofence.valid()) {
+		for (size_t i = 0; i < nMissionItems; i++) {
+			struct mission_item_s missionitem;
+			const ssize_t len = sizeof(missionitem);
+
+			if (dm_read(dm_current, i, &missionitem, len) != len) {
+				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+				return false;
+			}
+
+			if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) {
+				mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+
+bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
+{
+	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
+	for (size_t i = 0; i < nMissionItems; i++) {
+		static struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			if (throw_error) {
+				return false;
+			} else	{
+				return true;
+			}
+		}
+
+		if (home_alt > missionitem.altitude) {
+			if (throw_error) {
+				mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
+				return false;
+			} else	{
+				mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
+				return true;
+			}
+		}
+	}
+
+	return true;
+}
+
+bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
+{
+	/* Go through all mission items and search for a landing waypoint
+	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
+
+
+	for (size_t i = 0; i < nMissionItems; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(missionitem);
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND) {
+			struct mission_item_s missionitem_previous;
+			if (i != 0) {
+				if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+					return false;
+				}
+
+				float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
+				float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
+//				warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
+//						wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
+//				warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
+//						_nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
+
+				if (wp_distance > _nav_caps.landing_flare_length) {
+					/* Last wp is before flare region */
+
+					if (delta_altitude < 0) {
+						if (missionitem_previous.altitude <= slope_alt_req) {
+							/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
+							return true;
+						} else {
+							/* Landing waypoint is above altitude of slope at the given waypoint distance */
+							mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close");
+							mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
+									(double)(slope_alt_req),
+									(double)(wp_distance_req - wp_distance));
+							return false;
+						}
+					} else {
+						/* Landing waypoint is above last waypoint */
+						mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint");
+						return false;
+					}
+				} else {
+					/* Last wp is in flare region */
+					//xxx give recommendations
+					mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region");
+					return false;
+				}
+			} else {
+				mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint");
+				return false;
+			}
+		}
+	}
+
+
+//	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
+	return false;
+}
+
+void MissionFeasibilityChecker::updateNavigationCapabilities()
+{
+	(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+}
+
+void MissionFeasibilityChecker::init()
+{
+	if (!_initDone) {
+
+		_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+
+		_initDone = true;
+	}
+}
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/navigator/mission_feasibility_checker.h
index 431a435f7..96c9209d3 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -1,12 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -36,30 +30,57 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
-/*
- * @file multirotor_attitude_control.h
- *
- * Definition of attitude controller for multirotors.
+/**
+ * @file mission_feasibility_checker.h
+ * Provides checks if mission is feasible given the navigation capabilities
  *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  */
 
-#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
-#define MULTIROTOR_ATTITUDE_CONTROL_H_
+#ifndef MISSION_FEASIBILITY_CHECKER_H_
+#define MISSION_FEASIBILITY_CHECKER_H_
+
+#include <unistd.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/navigation_capabilities.h>
+#include <dataman/dataman.h>
+#include "geofence.h"
+
+
+class MissionFeasibilityChecker
+{
+private:
+	int		_mavlink_fd;
+
+	int _capabilities_sub;
+	struct navigation_capabilities_s _nav_caps;
+
+	bool _initDone;
+	void init();
+
+	/* Checks for all airframes */
+	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
+
+	/* Checks specific to fixedwing airframes */
+	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
+	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
+	void updateNavigationCapabilities();
+
+	/* Checks specific to rotarywing airframes */
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
+public:
+
+	MissionFeasibilityChecker();
+	~MissionFeasibilityChecker() {}
+
+	/*
+	 * Returns true if mission is feasible and false otherwise
+	 */
+	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
 
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
+};
 
-void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral);
 
-#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
+#endif /* MISSION_FEASIBILITY_CHECKER_H_ */
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
new file mode 100644
index 000000000..881caa24e
--- /dev/null
+++ b/src/modules/navigator/mission_params.c
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   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
+ */
+
+/**
+ * Take-off altitude
+ *
+ * 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);
+
+/**
+ * Enable persistent onboard mission storage
+ *
+ * When enabled, missions that have been uploaded by the GCS are stored
+ * and reloaded after reboot persistently.
+ *
+ * @min 0
+ * @max 1
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
+
+/**
+ * Maximal horizontal distance from home to first waypoint
+ *
+ * Failsafe check to prevent running mission stored from previous flight at a new takeoff location.
+ * Set a value of zero or less to disable. The mission will not be started if the current
+ * waypoint is more distant than MIS_DIS_1WP from the current position.
+ *
+ * @min 0
+ * @max 250
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175);
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 0404b06c7..637eaae59 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
@@ -38,4 +38,19 @@
 MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
-		  navigator_params.c
+		  navigator_params.c \
+		  navigator_mode.cpp \
+		  mission_block.cpp \
+		  mission.cpp \
+		  mission_params.c \
+		  loiter.cpp \
+		  rtl.cpp \
+		  rtl_params.c \
+		  offboard.cpp \
+		  mission_feasibility_checker.cpp \
+		  geofence.cpp \
+		  geofence_params.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
new file mode 100644
index 000000000..bf6e2ea0e
--- /dev/null
+++ b/src/modules/navigator/navigator.h
@@ -0,0 +1,226 @@
+/***************************************************************************
+ *
+ *   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 <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"
+#include "loiter.h"
+#include "rtl.h"
+#include "offboard.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 4
+
+class Navigator : public control::SuperBlock
+{
+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);
+
+	/**
+	 * Setters
+	 */
+	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
+	void		set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
+
+	/**
+	 * Getters
+	 */
+	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_control_mode_s*	    get_control_mode() { return &_control_mode; }
+	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
+	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	struct position_setpoint_triplet_s*		    get_position_setpoint_triplet() { return &_pos_sp_triplet; }
+	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
+	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+	int		get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
+	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_acceptance_radius() { return _param_acceptance_radius.get(); }
+	int		get_mavlink_fd() { return _mavlink_fd; }
+
+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		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_offboard_control_sp_sub;	/*** offboard control subscription */
+	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 */
+
+	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 */
+
+	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 */
+	Offboard	_offboard;			/**< class that handles offboard */
+
+	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
+
+	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
+	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
+
+	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
+	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
+	/**
+	 * 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();
+
+	/**
+	 * Update parameters
+	 */
+	void		params_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 f6c44444a..1a5ba4c1a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
+ *   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
@@ -32,13 +31,19 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_main.c
- * Implementation of the main navigation state machine.
+ * @file navigator_main.cpp
  *
- * Handles missions, geo fencing and failsafe navigation behavior.
+ * 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 <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -48,26 +53,30 @@
 #include <math.h>
 #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/airspeed.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.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 <systemlib/param/param.h>
+#include <uORB/topics/fence.h>
+#include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+
 #include <systemlib/err.h>
-#include <geo/geo.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 "navigator.h"
 
 /**
  * navigator app start / stop handling function
@@ -76,169 +85,55 @@
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator
-{
-public:
-	/**
-	 * Constructor
-	 */
-	Navigator();
-
-	/**
-	 * Destructor, also kills the sensors task.
-	 */
-	~Navigator();
-
-	/**
-	 * Start the sensors task.
-	 *
-	 * @return		OK on success.
-	 */
-	int		start();
-
-private:
-
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	int		_navigator_task;			/**< task handle for sensor task */
-
-	int		_global_pos_sub;
-	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_attitude_sub;			/**< raw rc channels data subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_mission_sub;
-
-	orb_advert_t	_triplet_pub;			/**< position setpoint */
-
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
-	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_status_s				_vstatus;		/**< vehicle status */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct vehicle_global_position_set_triplet_s	_global_triplet;	/**< triplet of global setpoints */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	unsigned	_mission_items_maxcount;				/**< maximum number of mission items supported */
-	struct	mission_item_s 				* _mission_items;	/**< storage for mission items */
-	bool		_mission_valid;						/**< flag if mission is valid */
-
-	/** manual control states */
-	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
-	float		_loiter_hold_lat;
-	float		_loiter_hold_lon;
-	float		_loiter_hold_alt;
-	bool		_loiter_hold;
-
-	struct {
-		float throttle_cruise;
-	}		_parameters;			/**< local copies of interesting parameters */
-
-	struct {
-		param_t throttle_cruise;
-
-	}		_parameter_handles;		/**< handles for interesting parameters */
-
-
-	/**
-	 * Update our local parameter cache.
-	 */
-	int		parameters_update();
-
-	/**
-	 * Update control outputs
-	 *
-	 */
-	void		control_update();
-
-	/**
-	 * Check for changes in vehicle status.
-	 */
-	void		vehicle_status_poll();
-
-	/**
-	 * Check for position updates.
-	 */
-	void		vehicle_attitude_poll();
-
-	/**
-	 * Check for set triplet updates.
-	 */
-	void		mission_poll();
-
-	/**
-	 * Control throttle.
-	 */
-	float		control_throttle(float energy_error);
-
-	/**
-	 * Control pitch.
-	 */
-	float		control_pitch(float altitude_error);
-
-	void calculate_airspeed_errors();
-	void calculate_gndspeed_undershoot();
-	void calculate_altitude_error();
-
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main sensor collection task.
-	 */
-	void		task_main() __attribute__((noreturn));
-};
 
 namespace navigator
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Navigator	*g_navigator;
 }
 
 Navigator::Navigator() :
-
+	SuperBlock(NULL, "NAV"),
 	_task_should_exit(false),
 	_navigator_task(-1),
-
-/* subscriptions */
+	_mavlink_fd(-1),
 	_global_pos_sub(-1),
-	_att_sub(-1),
-	_airspeed_sub(-1),
+	_home_pos_sub(-1),
 	_vstatus_sub(-1),
-	_params_sub(-1),
-	_manual_control_sub(-1),
-
-/* publications */
-	_triplet_pub(-1),
-
-/* performance counters */
+	_capabilities_sub(-1),
+	_offboard_control_sp_sub(-1),
+	_control_mode_sub(-1),
+	_onboard_mission_sub(-1),
+	_offboard_mission_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")),
-/* states */
-	_mission_items_maxcount(20),
-	_mission_valid(false),
-	_loiter_hold(false)
+	_geofence({}),
+	_geofence_violation_warning_sent(false),
+	_fence_valid(false),
+	_inside_fence(true),
+	_navigation_mode(nullptr),
+	_mission(this, "MIS"),
+	_loiter(this, "LOI"),
+	_rtl(this, "RTL"),
+	_offboard(this, "OFF"),
+	_param_loiter_radius(this, "LOITER_RAD"),
+	_param_acceptance_radius(this, "ACC_RAD")
 {
-	_mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount);
-	if (!_mission_items) {
-		_mission_items_maxcount = 0;
-		warnx("no free RAM to allocate mission, rejecting any waypoints");
-	}
-
-	_parameter_handles.throttle_cruise = param_find("NAV_DUMMY");
+	/* Create a list of our possible navigation types */
+	_navigation_mode_array[0] = &_mission;
+	_navigation_mode_array[1] = &_loiter;
+	_navigation_mode_array[2] = &_rtl;
+	_navigation_mode_array[3] = &_offboard;
 
-	/* fetch initial parameter values */
-	parameters_update();
+	updateParams();
 }
 
 Navigator::~Navigator()
@@ -266,70 +161,48 @@ Navigator::~Navigator()
 	navigator::g_navigator = nullptr;
 }
 
-int
-Navigator::parameters_update()
+void
+Navigator::global_position_update()
 {
-
-	//param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
-
-	return OK;
+	orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
 }
 
 void
-Navigator::vehicle_status_poll()
+Navigator::home_position_update()
 {
-	bool vstatus_updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_vstatus_sub, &vstatus_updated);
+	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+}
 
-	if (vstatus_updated) {
+void
+Navigator::navigation_capabilities_update()
+{
+	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+}
 
-		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
+void
+Navigator::vehicle_status_update()
+{
+	if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
+		/* in case the commander is not be running */
+		_vstatus.arming_state = ARMING_STATE_STANDBY;
 	}
 }
 
 void
-Navigator::vehicle_attitude_poll()
+Navigator::vehicle_control_mode_update()
 {
-	/* check if there is a new position */
-	bool att_updated;
-	orb_check(_att_sub, &att_updated);
-
-	if (att_updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+	if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) {
+		/* in case the commander is not be running */
+		_control_mode.flag_control_auto_enabled = false;
+		_control_mode.flag_armed = false;
 	}
 }
 
 void
-Navigator::mission_poll()
+Navigator::params_update()
 {
-	/* check if there is a new setpoint */
-	bool mission_updated;
-	orb_check(_mission_sub, &mission_updated);
-
-	if (mission_updated) {
-
-		struct mission_s mission;
-		orb_copy(ORB_ID(mission), _mission_sub, &mission);
-
-		// XXX this is not optimal yet, but a first prototype /
-		// test implementation
-
-		if (mission.count <= _mission_items_maxcount) {
-			/*
-			 * Perform an atomic copy & state update
-			 */
-			irqstate_t flags = irqsave();
-
-			memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s));
-			_mission_valid = true;
-
-			irqrestore(flags);
-		} else {
-			warnx("mission larger than storage space");
-		}
-	}
+	parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
 }
 
 void
@@ -341,196 +214,190 @@ Navigator::task_main_trampoline(int argc, char *argv[])
 void
 Navigator::task_main()
 {
-
 	/* inform about start */
 	warnx("Initializing..");
-	fflush(stdout);
 
-	/*
-	 * do subscriptions
-	 */
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	/* Try to load the geofence:
+	 * if /fs/microsd/etc/geofence.txt load from this file
+	 * else clear geofence data in datamanager */
+	struct stat buffer;
+
+	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
+		warnx("Try to load geofence.txt");
+		_geofence.loadFromFile(GEOFENCE_FILENAME);
+
+	} else {
+		if (_geofence.clearDm() > 0)
+			warnx("Geofence cleared");
+		else
+			warnx("Could not clear geofence");
+	}
+
+	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_mission_sub = orb_subscribe(ORB_ID(mission));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_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));
+	_offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+
+	/* copy all topics first time */
+	vehicle_status_update();
+	vehicle_control_mode_update();
+	global_position_update();
+	home_position_update();
+	navigation_capabilities_update();
+	params_update();
 
-	/* rate limit vehicle status updates to 5Hz */
-	orb_set_interval(_vstatus_sub, 200);
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	parameters_update();
+	hrt_abstime mavlink_open_time = 0;
+	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[2];
+	struct pollfd fds[6];
 
 	/* 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 = _capabilities_sub;
+	fds[2].events = POLLIN;
+	fds[3].fd = _vstatus_sub;
+	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) {
 
-		/* wait for up to 500ms for data */
+		/* 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)
+		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;
 		}
 
 		perf_begin(_loop_perf);
 
-		/* check vehicle status for changes to publication state */
-		vehicle_status_poll();
-
-		/* only update parameters if they changed */
-		if (fds[0].revents & POLLIN) {
-			/* read from param to clear updated flag */
-			struct parameter_update_s update;
-			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
-
-			/* update parameters from storage */
-			parameters_update();
+		if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
+			/* try to reopen the mavlink log device with specified interval */
+			mavlink_open_time = hrt_abstime() + mavlink_open_interval;
+			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
-		/* only run controller if position changed */
-		if (fds[1].revents & POLLIN) {
-
-
-			static uint64_t last_run = 0;
-			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-			last_run = hrt_absolute_time();
-
-			/* guard against too large deltaT's */
-			if (deltaT > 1.0f)
-				deltaT = 0.01f;
-
-			/* load local copies */
-			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
-
-			vehicle_attitude_poll();
-
-			mission_poll();
-
-			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
-			// Current waypoint
-			math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
-			// Global position
-			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
-
-			/* AUTONOMOUS FLIGHT */
-
-			if (1 /* autonomous flight */) {
-
-				/* execute navigation once we have a setpoint */
-				if (_mission_valid) {
-
-					// Next waypoint
-					math::Vector2f prev_wp;
-
-					if (_global_triplet.previous_valid) {
-						prev_wp.setX(_global_triplet.previous.lat / 1e7f);
-						prev_wp.setY(_global_triplet.previous.lon / 1e7f);
-
-					} else {
-						/*
-						 * No valid next waypoint, go for heading hold.
-						 * This is automatically handled by the L1 library.
-						 */
-						prev_wp.setX(_global_triplet.current.lat / 1e7f);
-						prev_wp.setY(_global_triplet.current.lon / 1e7f);
-
-					}
-
-
-
-					/********   MAIN NAVIGATION STATE MACHINE   ********/
-
-					// XXX to be put in its own class
+		/* parameters updated */
+		if (fds[5].revents & POLLIN) {
+			params_update();
+			updateParams();
+		}
 
-					if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
-						/* waypoint is a plain navigation waypoint */
-						
+		/* vehicle control mode updated */
+		if (fds[4].revents & POLLIN) {
+			vehicle_control_mode_update();
+		}
 
-					} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		/* vehicle status updated */
+		if (fds[3].revents & POLLIN) {
+			vehicle_status_update();
+		}
 
-						/* waypoint is a loiter waypoint */
-						
-					}
+		/* navigation capabilities updated */
+		if (fds[2].revents & POLLIN) {
+			navigation_capabilities_update();
+		}
 
-					// XXX at this point we always want no loiter hold if a
-					// mission is active
-					_loiter_hold = false;
+		/* home position updated */
+		if (fds[1].revents & POLLIN) {
+			home_position_update();
+		}
 
-				} else {
+		/* global position updated */
+		if (fds[0].revents & POLLIN) {
+			global_position_update();
 
-					if (!_loiter_hold) {
-						_loiter_hold_lat = _global_pos.lat / 1e7f;
-						_loiter_hold_lon = _global_pos.lon / 1e7f;
-						_loiter_hold_alt = _global_pos.alt;
-						_loiter_hold = true;
-					}
+			/* Check geofence violation */
+			if (!_geofence.inside(&_global_pos)) {
 
-					//_parameters.loiter_hold_radius
+				/* 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 if (0/* seatbelt mode enabled */) {
-
-				/** SEATBELT FLIGHT **/
-				continue;
-
 			} else {
-
-				/** MANUAL FLIGHT **/
-
-				/* no flight mode applies, do not publish an attitude setpoint */
-				continue;
+				/* Reset the _geofence_violation_warning_sent field */
+				_geofence_violation_warning_sent = false;
 			}
+		}
 
-			/********   MAIN NAVIGATION STATE MACHINE   ********/
-
-			if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-				// XXX define launch position and return
-
-			} else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) {
-				// XXX flared descent on final approach
-
-			} else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
-
-				/* apply minimum pitch if altitude has not yet been reached */
-				if (_global_pos.alt < _global_triplet.current.altitude) {
-					_att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1);
-				}
-			}
+		/* Do stuff according to navigation state set by commander */
+		switch (_vstatus.nav_state) {
+			case NAVIGATION_STATE_MANUAL:
+			case NAVIGATION_STATE_ACRO:
+			case NAVIGATION_STATE_ALTCTL:
+			case NAVIGATION_STATE_POSCTL:
+				_navigation_mode = nullptr;
+				_can_loiter_at_sp = false;
+				break;
+			case NAVIGATION_STATE_AUTO_MISSION:
+				_navigation_mode = &_mission;
+				break;
+			case NAVIGATION_STATE_AUTO_LOITER:
+				_navigation_mode = &_loiter;
+				break;
+			case NAVIGATION_STATE_AUTO_RTL:
+				_navigation_mode = &_rtl;
+				break;
+			case NAVIGATION_STATE_AUTO_RTGS:
+				_navigation_mode = &_rtl; /* TODO: change this to something else */
+				break;
+			case NAVIGATION_STATE_LAND:
+			case NAVIGATION_STATE_TERMINATION:
+			case NAVIGATION_STATE_OFFBOARD:
+				_navigation_mode = &_offboard;
+				break;
+			default:
+				_navigation_mode = nullptr;
+				_can_loiter_at_sp = false;
+				break;
+		}
 
-			/* lazily publish the setpoint only once available */
-			if (_triplet_pub > 0) {
-				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet);
+		/* iterate through navigation modes and set active/inactive for each */
+		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
+		}
 
-			} else {
-				/* advertise and publish */
-				_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet);
-			}
+		/* if nothing is running, set position setpoint triplet invalid */
+		if (_navigation_mode == nullptr) {
+			// TODO publish empty sp only once
+			_pos_sp_triplet.previous.valid = false;
+			_pos_sp_triplet.current.valid = false;
+			_pos_sp_triplet.next.valid = false;
+			_pos_sp_triplet_updated = true;
+		}
 
+		if (_pos_sp_triplet_updated) {
+			publish_position_setpoint_triplet();
+			_pos_sp_triplet_updated = false;
 		}
 
 		perf_end(_loop_perf);
 	}
-
-	warnx("exiting.\n");
+	warnx("exiting.");
 
 	_navigator_task = -1;
 	_exit(0);
@@ -543,11 +410,11 @@ Navigator::start()
 
 	/* start the task */
 	_navigator_task = task_spawn_cmd("navigator",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2048,
-				       (main_t)&Navigator::task_main_trampoline,
-				       nullptr);
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 2000,
+					 (main_t)&Navigator::task_main_trampoline,
+					 nullptr);
 
 	if (_navigator_task < 0) {
 		warn("task start failed");
@@ -557,20 +424,81 @@ Navigator::start()
 	return OK;
 }
 
+void
+Navigator::status()
+{
+	/* 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 (_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);
+
+	} else {
+		warnx("Geofence not set");
+	}
+}
+
+void
+Navigator::publish_position_setpoint_triplet()
+{
+	/* update navigation state */
+	/* TODO: set nav_state */
+
+	/* lazily publish the position setpoint triplet only once available */
+	if (_pos_sp_triplet_pub > 0) {
+		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
+
+	} else {
+		_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
+	}
+}
+
+void Navigator::add_fence_point(int argc, char *argv[])
+{
+	_geofence.addPoint(argc, argv);
+}
+
+void Navigator::load_fence_from_file(const char *filename)
+{
+	_geofence.loadFromFile(filename);
+}
+
+
+static void usage()
+{
+	errx(1, "usage: navigator {start|stop|status|fence|fencefile}");
+}
+
 int navigator_main(int argc, char *argv[])
 {
-	if (argc < 1)
-		errx(1, "usage: navigator {start|stop|status}");
+	if (argc < 2) {
+		usage();
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (navigator::g_navigator != nullptr)
+		if (navigator::g_navigator != nullptr) {
 			errx(1, "already running");
+		}
 
 		navigator::g_navigator = new Navigator;
 
-		if (navigator::g_navigator == nullptr)
+		if (navigator::g_navigator == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != navigator::g_navigator->start()) {
 			delete navigator::g_navigator;
@@ -578,27 +506,28 @@ int navigator_main(int argc, char *argv[])
 			err(1, "start failed");
 		}
 
-		exit(0);
+		return 0;
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		if (navigator::g_navigator == nullptr)
-			errx(1, "not running");
+	if (navigator::g_navigator == nullptr)
+		errx(1, "not running");
 
+	if (!strcmp(argv[1], "stop")) {
 		delete navigator::g_navigator;
 		navigator::g_navigator = nullptr;
-		exit(0);
-	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (navigator::g_navigator) {
-			errx(0, "running");
+	} else if (!strcmp(argv[1], "status")) {
+		navigator::g_navigator->status();
 
-		} else {
-			errx(1, "not running");
-		}
+	} else if (!strcmp(argv[1], "fence")) {
+		navigator::g_navigator->add_fence_point(argc - 2, argv + 2);
+
+	} else if (!strcmp(argv[1], "fencefile")) {
+		navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
+
+	} else {
+		usage();
 	}
 
-	warnx("unrecognized command");
-	return 1;
+	return 0;
 }
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
new file mode 100644
index 000000000..f43215665
--- /dev/null
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   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
+ *
+ * Base class for different modes in navigator
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "navigator_mode.h"
+#include "navigator.h"
+
+NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
+	SuperBlock(NULL, name),
+	_navigator(navigator),
+	_first_run(true)
+{
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	on_inactive();
+}
+
+NavigatorMode::~NavigatorMode()
+{
+}
+
+void
+NavigatorMode::run(bool active) {
+	if (active) {
+		if (_first_run) {
+			/* first run */
+			_first_run = false;
+			on_activation();
+
+		} else {
+			/* periodic updates when active */
+			on_active();
+		}
+
+	} else {
+		/* periodic updates when inactive */
+		_first_run = true;
+		on_inactive();
+	}
+}
+
+void
+NavigatorMode::on_inactive()
+{
+}
+
+void
+NavigatorMode::on_activation()
+{
+	/* invalidate position setpoint by default */
+	_navigator->get_position_setpoint_triplet()->current.valid = false;
+}
+
+void
+NavigatorMode::on_active()
+{
+}
diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/modules/navigator/navigator_mode.h
index df8970d3a..a7ba79bba 100644
--- a/src/lib/mathlib/math/Dcm.hpp
+++ b/src/modules/navigator/navigator_mode.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -30,79 +30,64 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 /**
- * @file Dcm.hpp
+ * @file navigator_mode.h
+ *
+ * Base class for different modes in navigator
  *
- * math direction cosine matrix
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-//#pragma once
+#ifndef NAVIGATOR_MODE_H
+#define NAVIGATOR_MODE_H
 
-#include "Vector.hpp"
-#include "Matrix.hpp"
+#include <drivers/drv_hrt.h>
 
-namespace math
-{
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
 
-class Quaternion;
-class EulerAngles;
+#include <dataman/dataman.h>
 
-/**
- * This is a Tait Bryan, Body 3-2-1 sequence.
- * (yaw)-(pitch)-(roll)
- * The Dcm transforms a vector in the body frame
- * to the navigation frame, typically represented
- * as C_nb. C_bn can be obtained through use
- * of the transpose() method.
- */
-class __EXPORT Dcm : public Matrix
+#include <uORB/topics/position_setpoint_triplet.h>
+
+class Navigator;
+
+class NavigatorMode : public control::SuperBlock
 {
 public:
 	/**
-	 * default ctor
+	 * Constructor
 	 */
-	Dcm();
+	NavigatorMode(Navigator *navigator, const char *name);
 
 	/**
-	 * scalar ctor
+	 * Destructor
 	 */
-	Dcm(float c00, float c01, float c02,
-	    float c10, float c11, float c12,
-	    float c20, float c21, float c22);
+	virtual ~NavigatorMode();
 
-	/**
-	 * data ctor
-	 */
-	Dcm(const float *data);
+	void run(bool active);
 
 	/**
-	 * array ctor
+	 * This function is called while the mode is inactive
 	 */
-	Dcm(const float data[3][3]);
+	virtual void on_inactive();
 
 	/**
-	 * quaternion ctor
+	 * This function is called one time when mode become active, poos_sp_triplet must be initialized here
 	 */
-	Dcm(const Quaternion &q);
+	virtual void on_activation();
 
 	/**
-	 * euler angles ctor
+	 * This function is called while the mode is active
 	 */
-	Dcm(const EulerAngles &euler);
+	virtual void on_active();
 
-	/**
-	 * copy ctor (deep)
-	 */
-	Dcm(const Dcm &right);
+protected:
+	Navigator *_navigator;
 
-	/**
-	 * dtor
-	 */
-	virtual ~Dcm();
+private:
+	bool _first_run;
 };
 
-int __EXPORT dcmTest();
-
-} // math
-
+#endif
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 06df9a452..084afe340 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -18,7 +17,7 @@
  *    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
+ * 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,
@@ -35,19 +34,33 @@
 /**
  * @file navigator_params.c
  *
- * Parameters defined by the navigator task.
+ * Parameters for navigator in general
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <nuttx/config.h>
 
 #include <systemlib/param/param.h>
 
-/*
- * Navigator parameters, accessible via MAVLink
+/**
+ * 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);
 
-PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f);
-
+/**
+ * Acceptance Radius
+ *
+ * Default acceptance radius, overridden by acceptance radius of waypoint if set.
+ *
+ * @unit meters
+ * @min 1.0
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp
new file mode 100644
index 000000000..dcb5c6000
--- /dev/null
+++ b/src/modules/navigator/offboard.cpp
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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 offboard.cpp
+ *
+ * Helper class for offboard commands
+ *
+ * @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 "navigator.h"
+#include "offboard.h"
+
+Offboard::Offboard(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	_offboard_control_sp({0})
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+Offboard::~Offboard()
+{
+}
+
+void
+Offboard::on_inactive()
+{
+}
+
+void
+Offboard::on_activation()
+{
+}
+
+void
+Offboard::on_active()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	bool updated;
+	orb_check(_navigator->get_offboard_control_sp_sub(), &updated);
+	if (updated) {
+		update_offboard_control_setpoint();
+	}
+
+	/* copy offboard setpoints to the corresponding topics */
+	if (_navigator->get_control_mode()->flag_control_position_enabled
+	    && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
+		/* position control */
+		pos_sp_triplet->current.x = _offboard_control_sp.p1;
+		pos_sp_triplet->current.y = _offboard_control_sp.p2;
+		pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
+		pos_sp_triplet->current.z = -_offboard_control_sp.p4;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->current.position_valid = true;
+
+		_navigator->set_position_setpoint_triplet_updated();
+
+	} else if (_navigator->get_control_mode()->flag_control_velocity_enabled
+	           && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) {
+		/* velocity control */
+		pos_sp_triplet->current.vx = _offboard_control_sp.p2;
+		pos_sp_triplet->current.vy = _offboard_control_sp.p1;
+		pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
+		pos_sp_triplet->current.vz = _offboard_control_sp.p4;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->current.velocity_valid = true;
+
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+
+}
+
+
+void
+Offboard::update_offboard_control_setpoint()
+{
+	orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp);
+
+}
diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/modules/navigator/offboard.h
index 568d9669a..66b923bdb 100644
--- a/src/lib/mathlib/math/Vector3.hpp
+++ b/src/modules/navigator/offboard.h
@@ -1,6 +1,6 @@
-/****************************************************************************
+/***************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -30,47 +30,43 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 /**
- * @file Vector3.hpp
+ * @file offboard.h
+ *
+ * Helper class for offboard commands
  *
- * math 3 vector
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#pragma once
+#ifndef NAVIGATOR_OFFBOARD_H
+#define NAVIGATOR_OFFBOARD_H
 
-#include "Vector.hpp"
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
 
-namespace math
-{
+#include <uORB/uORB.h>
+#include <uORB/topics/offboard_control_setpoint.h>
 
-class __EXPORT Vector3 :
-	public Vector
+#include "navigator_mode.h"
+
+class Navigator;
+
+class Offboard : public NavigatorMode
 {
 public:
-	Vector3();
-	Vector3(const Vector &right);
-	Vector3(float x, float y, float z);
-	Vector3(const float *data);
-	virtual ~Vector3();
-	Vector3 cross(const Vector3 &b) const;
+	Offboard(Navigator *navigator, const char *name);
 
-	/**
-	 * accessors
-	 */
-	void setX(float x) { (*this)(0) = x; }
-	void setY(float y) { (*this)(1) = y; }
-	void setZ(float z) { (*this)(2) = z; }
-	const float &getX() const { return (*this)(0); }
-	const float &getY() const { return (*this)(1); }
-	const float &getZ() const { return (*this)(2); }
-};
-    
-class __EXPORT Vector3f :
-	public Vector3
-{
-};
+	~Offboard();
+
+	virtual void on_inactive();
 
-int __EXPORT vector3Test();
-} // math
+	virtual void on_activation();
+
+	virtual void on_active();
+private:
+	void update_offboard_control_setpoint();
+
+	struct offboard_control_setpoint_s _offboard_control_sp;
+};
 
+#endif
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
new file mode 100644
index 000000000..142a73409
--- /dev/null
+++ b/src/modules/navigator/rtl.cpp
@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   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 <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "rtl.h"
+
+#define DELAY_SIGMA	0.01f
+
+RTL::RTL(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_rtl_state(RTL_STATE_NONE),
+	_param_return_alt(this, "RETURN_ALT"),
+	_param_descend_alt(this, "DESCEND_ALT"),
+	_param_land_delay(this, "LAND_DELAY")
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+RTL::~RTL()
+{
+}
+
+void
+RTL::on_inactive()
+{
+	/* reset RTL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_rtl_state = RTL_STATE_NONE;
+	}
+}
+
+void
+RTL::on_activation()
+{
+	/* 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_LANDED;
+			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();
+}
+
+void
+RTL::on_active()
+{
+	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
+		advance_rtl();
+		set_rtl_item();
+	}
+}
+
+void
+RTL::set_rtl_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* make sure we have the latest params */
+	updateParams();
+
+	set_previous_pos_setpoint();
+	_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;
+		_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 = _navigator->get_acceptance_radius();
+		_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: climb to %d meters above home",
+			(int)(climb_alt - _navigator->get_home_position()->alt));
+		break;
+	}
+
+	case RTL_STATE_RETURN: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->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(
+		 	        _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;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_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: return at %d meters above home",
+			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
+		break;
+	}
+
+	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;
+		_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 = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = false;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		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_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 = _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;
+		_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", (double)_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;
+		_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 = _navigator->get_acceptance_radius();
+		_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: land at home");
+		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 = _navigator->get_acceptance_radius();
+		_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;
+	}
+
+	reset_mission_item_reached();
+
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+RTL::advance_rtl()
+{
+	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() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
+			_rtl_state = RTL_STATE_LOITER;
+
+		} else {
+			_rtl_state = RTL_STATE_LAND;
+		}
+		break;
+
+	case RTL_STATE_LOITER:
+		_rtl_state = RTL_STATE_LAND;
+		break;
+
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_LANDED;
+		break;
+
+	default:
+		break;
+	}
+}
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h b/src/modules/navigator/rtl.h
index 600e35b89..5928f1f07 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h
+++ b/src/modules/navigator/rtl.h
@@ -1,8 +1,6 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@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
@@ -32,20 +30,67 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+/**
+ * @file navigator_rtl.h
+ * Helper class for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
 
-/* @file Fixed Wing Attitude Control */
+#ifndef NAVIGATOR_RTL_H
+#define NAVIGATOR_RTL_H
 
-#ifndef FIXEDWING_ATT_CONTROL_ATT_H_
-#define FIXEDWING_ATT_CONTROL_ATT_H_
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
 
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 
-int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				   const struct vehicle_attitude_s *att,
-				   const float speed_body[],
-				   struct vehicle_rates_setpoint_s *rates_sp);
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class RTL : public MissionBlock
+{
+public:
+	RTL(Navigator *navigator, const char *name);
+
+	~RTL();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/**
+	 * Set the RTL item
+	 */
+	void		set_rtl_item();
+
+	/**
+	 * Move to next RTL item
+	 */
+	void		advance_rtl();
+
+	enum RTLState {
+		RTL_STATE_NONE = 0,
+		RTL_STATE_CLIMB,
+		RTL_STATE_RETURN,
+		RTL_STATE_DESCEND,
+		RTL_STATE_LOITER,
+		RTL_STATE_LAND,
+		RTL_STATE_LANDED,
+	} _rtl_state;
+
+	control::BlockParamFloat _param_return_alt;
+	control::BlockParamFloat _param_descend_alt;
+	control::BlockParamFloat _param_land_delay;
+};
 
-#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
+#endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
new file mode 100644
index 000000000..bfe6ce7e1
--- /dev/null
+++ b/src/modules/navigator/rtl_params.c
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   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
+ */
+
+/**
+ * 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
+ *
+ * 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/position_estimator/position_estimator_main.c b/src/modules/position_estimator/position_estimator_main.c
deleted file mode 100644
index e84945299..000000000
--- a/src/modules/position_estimator/position_estimator_main.c
+++ /dev/null
@@ -1,423 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *			 Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           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
- * 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 position_estimator_main.c
- * Model-identification based position estimator for multirotors
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <float.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <limits.h>
-#include <math.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <poll.h>
-
-#define N_STATES 6
-#define ERROR_COVARIANCE_INIT 3
-#define R_EARTH 6371000.0
-
-#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
-#define REPROJECTION_COUNTER_LIMIT 125
-
-__EXPORT int position_estimator_main(int argc, char *argv[]);
-
-static uint16_t position_estimator_counter_position_information;
-
-/* values for map projection */
-static double phi_1;
-static double sin_phi_1;
-static double cos_phi_1;
-static double lambda_0;
-static double scale;
-
-/**
- * Initializes the map transformation.
- *
- * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
-{
-	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	phi_1 = lat_0 / 180.0 * M_PI;
-	lambda_0 = lon_0 / 180.0 * M_PI;
-
-	sin_phi_1 = sin(phi_1);
-	cos_phi_1 = cos(phi_1);
-
-	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
-
-	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-	const double r_earth = 6371000;
-
-	double lat1 = phi_1;
-	double lon1 = lambda_0;
-
-	double lat2 = phi_1 + 0.5 / 180 * M_PI;
-	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
-	double sin_lat_2 = sin(lat2);
-	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
-
-	/* 2) calculate distance rho on plane */
-	double k_bar = 0;
-	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
-	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
-	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
-
-	scale = d / rho;
-
-}
-
-/**
- * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-static void map_projection_project(double lat, double lon, float *x, float *y)
-{
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	double phi = lat / 180.0 * M_PI;
-	double lambda = lon / 180.0 * M_PI;
-
-	double sin_phi = sin(phi);
-	double cos_phi = cos(phi);
-
-	double k_bar = 0;
-	/* using small angle approximation (formula in comment is without aproximation) */
-	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	/* using small angle approximation (formula in comment is without aproximation) */
-	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
-	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
-
-//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
-}
-
-/**
- * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
- *
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-static void map_projection_reproject(float x, float y, double *lat, double *lon)
-{
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-
-	double x_descaled = x / scale;
-	double y_descaled = y / scale;
-
-	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
-	double sin_c = sin(c);
-	double cos_c = cos(c);
-
-	double lat_sphere = 0;
-
-	if (c != 0)
-		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
-	else
-		lat_sphere = asin(cos_c * sin_phi_1);
-
-//	printf("lat_sphere = %.10f\n",lat_sphere);
-
-	double lon_sphere = 0;
-
-	if (phi_1  == M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
-
-	} else if (phi_1 == -M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
-
-	} else {
-
-		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
-		//using small angle approximation
-//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
-//    	if(denominator != 0)
-//    	{
-//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
-//    	}
-//    	else
-//    	{
-//    	...
-//    	}
-	}
-
-//	printf("lon_sphere = %.10f\n",lon_sphere);
-
-	*lat = lat_sphere * 180.0 / M_PI;
-	*lon = lon_sphere * 180.0 / M_PI;
-
-}
-
-/****************************************************************************
- * main
- ****************************************************************************/
-
-int position_estimator_main(int argc, char *argv[])
-{
-
-	/* welcome user */
-	printf("[multirotor position_estimator] started\n");
-
-	/* initialize values */
-	static float u[2] = {0, 0};
-	static float z[3] = {0, 0, 0};
-	static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0};
-	static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0
-						 };
-
-	static float xapo1[N_STATES];
-	static float Papo1[36];
-
-	static float gps_covariance[3] = {0.0f, 0.0f, 0.0f};
-
-	static uint16_t counter = 0;
-	position_estimator_counter_position_information = 0;
-
-	uint8_t predict_only = 1;
-
-	bool gps_valid = false;
-
-	bool new_initialization = true;
-
-	static double lat_current = 0.0d;//[°]] --> 47.0
-	static double lon_current = 0.0d; //[°]] -->8.5
-	float alt_current = 0.0f;
-
-
-	//TODO: handle flight without gps but with estimator
-
-	/* subscribe to vehicle status, attitude, gps */
-	struct vehicle_gps_position_s gps;
-	gps.fix_type = 0;
-	struct vehicle_status_s vstatus;
-	struct vehicle_attitude_s att;
-
-	int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* subscribe to attitude at 100 Hz */
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-
-	/* wait until gps signal turns valid, only then can we initialize the projection */
-	while (gps.fix_type < 3) {
-		struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} };
-
-		/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
-		 * this choice is critical, since the vehicle status might not
-		 * actually change, if this app is started after GPS lock was
-		 * aquired.
-		 */
-		if (poll(fds, 1, 5000)) {
-			/* Wait for the GPS update to propagate (we have some time) */
-			usleep(5000);
-			/* Read wether the vehicle status changed */
-			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-			gps_valid = (gps.fix_type > 2);
-		}
-	}
-
-	/* get gps value for first initialization */
-	orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-	lat_current = ((double)(gps.lat)) * 1e-7;
-	lon_current = ((double)(gps.lon)) * 1e-7;
-	alt_current = gps.alt * 1e-3;
-
-	/* initialize coordinates */
-	map_projection_init(lat_current, lon_current);
-
-	/* publish global position messages only after first GPS message */
-	struct vehicle_local_position_s local_pos = {
-		.x = 0,
-		.y = 0,
-		.z = 0
-	};
-	orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
-
-	printf("[multirotor position estimator] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
-
-	while (1) {
-
-		/*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */
-		struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} };
-
-		if (poll(fds, 1, 5000) <= 0) {
-			/* error / timeout */
-		} else {
-
-			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-			/* got attitude, updating pos as well */
-			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
-
-			/*copy attitude */
-			u[0] = att.roll;
-			u[1] = att.pitch;
-
-			/* initialize map projection with the last estimate (not at full rate) */
-			if (gps.fix_type > 2) {
-				/* Project gps lat lon (Geographic coordinate system) to plane*/
-				map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1]));
-
-				local_pos.x = z[0];
-				local_pos.y = z[1];
-				/* negative offset from initialization altitude */
-				local_pos.z = alt_current - (gps.alt) * 1e-3;
-
-
-				orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
-			}
-
-
-			// 	gps_covariance[0] = gps.eph; //TODO: needs scaling
-			// 	gps_covariance[1] = gps.eph;
-			// 	gps_covariance[2] = gps.epv;
-
-			// } else {
-			// 	/* we can not use the gps signal (it is of low quality) */
-			// 	predict_only = 1;
-			// }
-
-			// //		predict_only = 0;																//TODO: only for testing, removeme, XXX
-			// //		z[0] = sinf(((float)counter)/180.0f*3.14159265f);								//TODO: only for testing, removeme, XXX
-			// //		usleep(100000);																	//TODO: only for testing, removeme, XXX
-
-
-			// /*Get new estimation (this is calculated in the plane) */
-			// //TODO: if new_initialization == true: use 0,0,0, else use xapo
-			// if (true == new_initialization) {																								//TODO,XXX: uncomment!
-			// 	xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
-			// 	xapo[2] = 0;
-			// 	xapo[4] = 0;
-			// 	position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
-
-			// } else {
-			// 	position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
-			// }
-
-
-
-			// /* Copy values from xapo1 to xapo */
-			// int i;
-
-			// for (i = 0; i < N_STATES; i++) {
-			// 	xapo[i] = xapo1[i];
-			// }
-
-			// if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
-			// 	/* Reproject from plane to geographic coordinate system */
-			// 	//		map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
-			// 	map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
-			// 	//		//DEBUG
-			// 	//			if(counter%500 == 0)
-			// 	//			{
-			// 	//				printf("phi_1: %.10f\n", phi_1);
-			// 	//				printf("lambda_0: %.10f\n", lambda_0);
-			// 	//				printf("lat_estimated: %.10f\n", lat_current);
-			// 	//				printf("lon_estimated: %.10f\n", lon_current);
-			// 	//				printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
-			// 	//				fflush(stdout);
-			// 	//
-			// 	//			}
-
-			// 	//			if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
-			// 	//			{
-			// 	/* send out */
-
-			// 	global_pos.lat = lat_current;
-			// 	global_pos.lon = lon_current;
-			// 	global_pos.alt = xapo1[4];
-			// 	global_pos.vx = xapo1[1];
-			// 	global_pos.vy = xapo1[3];
-			// 	global_pos.vz = xapo1[5];
-
-				/* publish current estimate */
-				// orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
-				//			}
-				//			else
-				//			{
-				//				printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]);
-				//				fflush(stdout);
-				//			}
-
-			// }
-
-			counter++;
-		}
-
-	}
-
-	return 0;
-}
-
-
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
index 13328edb4..a36a4688d 100644
--- a/src/modules/position_estimator_inav/inertial_filter.c
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -5,27 +5,29 @@
  *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
  */
 
+#include <math.h>
+
 #include "inertial_filter.h"
 
-void inertial_filter_predict(float dt, float x[3])
+void inertial_filter_predict(float dt, float x[2], float acc)
 {
-	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
-	x[1] += x[2] * dt;
+	if (isfinite(dt)) {
+		if (!isfinite(acc)) {
+			acc = 0.0f;
+		}
+		x[0] += x[1] * dt + acc * dt * dt / 2.0f;
+		x[1] += acc * dt;
+	}
 }
 
-void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
+void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
 {
-	float ewdt = w * dt;
-	if (ewdt > 1.0f)
-		ewdt = 1.0f;	// prevent over-correcting
-	ewdt *= e;
-	x[i] += ewdt;
-
-	if (i == 0) {
-		x[1] += w * ewdt;
-		x[2] += w * w * ewdt / 3.0;
+	if (isfinite(e) && isfinite(w) && isfinite(dt)) {
+		float ewdt = e * w * dt;
+		x[i] += ewdt;
 
-	} else if (i == 1) {
-		x[2] += w * ewdt;
+		if (i == 0) {
+			x[1] += w * ewdt;
+		}
 	}
 }
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
index 761c17097..cdeb4cfc6 100644
--- a/src/modules/position_estimator_inav/inertial_filter.h
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -8,6 +8,6 @@
 #include <stdbool.h>
 #include <drivers/drv_hrt.h>
 
-void inertial_filter_predict(float dt, float x[3]);
+void inertial_filter_predict(float dt, float x[3], float acc);
 
 void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
index 939d76849..0658d3f09 100644
--- a/src/modules/position_estimator_inav/module.mk
+++ b/src/modules/position_estimator_inav/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 	= position_estimator_inav
 SRCS		 	= position_estimator_inav_main.c \
 			position_estimator_inav_params.c \
 			inertial_filter.c
+
+MODULE_STACKSIZE = 1200
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 3084b6d92..05eae047c 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.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,6 +34,8 @@
 /**
  * @file position_estimator_inav_main.c
  * Model-identification based position estimator for multirotors
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <unistd.h>
@@ -42,15 +43,13 @@
 #include <stdio.h>
 #include <stdbool.h>
 #include <fcntl.h>
-#include <float.h>
 #include <string.h>
 #include <nuttx/config.h>
 #include <nuttx/sched.h>
 #include <sys/prctl.h>
 #include <termios.h>
-#include <errno.h>
-#include <limits.h>
 #include <math.h>
+#include <float.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/actuator_controls.h>
@@ -60,6 +59,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/home_position.h>
 #include <uORB/topics/optical_flow.h>
 #include <mavlink/mavlink_log.h>
 #include <poll.h>
@@ -71,15 +71,22 @@
 #include "position_estimator_inav_params.h"
 #include "inertial_filter.h"
 
+#define MIN_VALID_W 0.00001f
+#define PUB_INTERVAL 10000	// limit publish rate to 100 Hz
+#define EST_BUF_SIZE 250000 / PUB_INTERVAL		// buffer size is 0.5s
+
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
 static int position_estimator_inav_task; /**< Handle of deamon task / thread */
 static bool verbose_mode = false;
 
-static const hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
-static const hrt_abstime flow_timeout = 1000000;	// optical flow timeout = 1s
+static const hrt_abstime gps_topic_timeout = 500000;		// GPS topic timeout = 0.5s
+static const hrt_abstime flow_topic_timeout = 1000000;	// optical flow topic timeout = 1s
+static const hrt_abstime sonar_timeout = 150000;	// sonar timeout = 150ms
+static const hrt_abstime sonar_valid_timeout = 1000000;	// estimate sonar distance during this time after sonar loss
+static const hrt_abstime xy_src_timeout = 2000000;	// estimate position during this time after position sources loss
 static const uint32_t updates_counter_len = 1000000;
-static const uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
+static const float max_flow = 1.0f;	// max flow value that can be used, rad/s
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
@@ -87,16 +94,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]);
 
 static void usage(const char *reason);
 
+static inline int min(int val1, int val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+static inline int max(int val1, int val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
 /**
  * Print the correct usage.
  */
 static void usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", reason);
+	}
 
-	fprintf(stderr,
-		"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
 	exit(1);
 }
 
@@ -110,12 +127,13 @@ static void usage(const char *reason)
  */
 int position_estimator_inav_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 		if (thread_running) {
-			printf("position_estimator_inav already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -123,28 +141,36 @@ int position_estimator_inav_main(int argc, char *argv[])
 		verbose_mode = false;
 
 		if (argc > 1)
-			if (!strcmp(argv[2], "-v"))
+			if (!strcmp(argv[2], "-v")) {
 				verbose_mode = true;
+			}
 
 		thread_should_exit = false;
 		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
-					       SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
+					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
 					       position_estimator_inav_thread_main,
 					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
+		if (thread_running) {
+			warnx("stop");
+			thread_should_exit = true;
+
+		} else {
+			warnx("app not started");
+		}
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tposition_estimator_inav is running\n");
+			warnx("app is running");
 
 		} else {
-			printf("\tposition_estimator_inav not started\n");
+			warnx("app not started");
 		}
 
 		exit(0);
@@ -154,31 +180,120 @@ int position_estimator_inav_main(int argc, char *argv[])
 	exit(1);
 }
 
+static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
+{
+	FILE *f = fopen("/fs/microsd/inav.log", "a");
+
+	if (f) {
+		char *s = malloc(256);
+		unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n",
+                              hrt_absolute_time(), msg, (double)dt,
+                              (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],
+                              (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]);
+		fwrite(s, 1, n, f);
+		n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n",
+                     (double)acc[0], (double)acc[1], (double)acc[2],
+                     (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1],
+                     (double)w_xy_gps_p, (double)w_xy_gps_v);
+		fwrite(s, 1, n, f);
+		free(s);
+	}
+
+	fsync(fileno(f));
+	fclose(f);
+}
+
 /****************************************************************************
  * main
  ****************************************************************************/
 int position_estimator_inav_thread_main(int argc, char *argv[])
 {
-	warnx("started.");
+	warnx("started");
 	int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	mavlink_log_info(mavlink_fd, "[inav] started");
 
-	/* initialize values */
-	float x_est[3] = { 0.0f, 0.0f, 0.0f };
-	float y_est[3] = { 0.0f, 0.0f, 0.0f };
-	float z_est[3] = { 0.0f, 0.0f, 0.0f };
+	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
+	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
+	float z_est[2] = { 0.0f, 0.0f };	// pos, vel
+
+	float est_buf[EST_BUF_SIZE][3][2];	// estimated position buffer
+	float R_buf[EST_BUF_SIZE][3][3];	// rotation matrix buffer
+	float R_gps[3][3];					// rotation matrix for GPS correction moment
+	memset(est_buf, 0, sizeof(est_buf));
+	memset(R_buf, 0, sizeof(R_buf));
+	memset(R_gps, 0, sizeof(R_gps));
+	int buf_ptr = 0;
+
+	static const float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
+	static const float max_eph_epv = 20.0f;	// max EPH/EPV acceptable for estimation
+
+	float eph = max_eph_epv;
+	float epv = 1.0f;
+
+	float eph_flow = 1.0f;
+
+	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
+	memset(x_est_prev, 0, sizeof(x_est_prev));
+	memset(y_est_prev, 0, sizeof(y_est_prev));
+	memset(z_est_prev, 0, sizeof(z_est_prev));
 
 	int baro_init_cnt = 0;
 	int baro_init_num = 200;
-	float baro_alt0 = 0.0f; /* to determine while start up */
+	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
+	float surface_offset = 0.0f;	// ground level offset from reference altitude
+	float surface_offset_rate = 0.0f;	// surface offset change rate
 	float alt_avg = 0.0f;
 	bool landed = true;
 	hrt_abstime landed_time = 0;
-	bool flag_armed = false;
 
-	uint32_t accel_counter = 0;
-	uint32_t baro_counter = 0;
+	hrt_abstime accel_timestamp = 0;
+	hrt_abstime baro_timestamp = 0;
+
+	bool ref_inited = false;
+	hrt_abstime ref_init_start = 0;
+	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
+	struct map_projection_reference_s ref;
+	memset(&ref, 0, sizeof(ref));
+	hrt_abstime home_timestamp = 0;
+
+	uint16_t accel_updates = 0;
+	uint16_t baro_updates = 0;
+	uint16_t gps_updates = 0;
+	uint16_t attitude_updates = 0;
+	uint16_t flow_updates = 0;
+
+	hrt_abstime updates_counter_start = hrt_absolute_time();
+	hrt_abstime pub_last = hrt_absolute_time();
+
+	hrt_abstime t_prev = 0;
+
+	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
+	float acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
+	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
+	float corr_baro = 0.0f;		// D
+	float corr_gps[3][2] = {
+		{ 0.0f, 0.0f },		// N (pos, vel)
+		{ 0.0f, 0.0f },		// E (pos, vel)
+		{ 0.0f, 0.0f },		// D (pos, vel)
+	};
+	float w_gps_xy = 1.0f;
+	float w_gps_z = 1.0f;
+	float corr_sonar = 0.0f;
+	float corr_sonar_filtered = 0.0f;
+
+	float corr_flow[] = { 0.0f, 0.0f };	// N E
+	float w_flow = 0.0f;
+
+	float sonar_prev = 0.0f;
+	hrt_abstime flow_prev = 0;			// time of last flow measurement
+	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
+	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)
+
+	bool gps_valid = false;			// GPS is valid
+	bool sonar_valid = false;		// sonar is valid
+	bool flow_valid = false;		// flow is valid
+	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)
 
 	/* declare and safely initialize all structs */
 	struct actuator_controls_s actuator;
@@ -189,6 +304,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(&sensor, 0, sizeof(sensor));
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
+	struct home_position_s home;
+	memset(&home, 0, sizeof(home));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_local_position_s local_pos;
@@ -206,10 +323,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	int home_position_sub = orb_subscribe(ORB_ID(home_position));
 
 	/* advertise */
 	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
-	orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+	orb_advert_t vehicle_global_position_pub = -1;
 
 	struct position_estimator_inav_params params;
 	struct position_estimator_inav_param_handles pos_inav_param_handles;
@@ -242,80 +360,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (wait_baro && sensor.baro_counter != baro_counter) {
-					baro_counter = sensor.baro_counter;
+				if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
+					baro_timestamp = sensor.baro_timestamp;
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
-						baro_alt0 += sensor.baro_alt_meter;
-						baro_init_cnt++;
+						if (isfinite(sensor.baro_alt_meter)) {
+							baro_offset += sensor.baro_alt_meter;
+							baro_init_cnt++;
+						}
 
 					} else {
 						wait_baro = false;
-						baro_alt0 /= (float) baro_init_cnt;
-						warnx("init baro: alt = %.3f", baro_alt0);
-						mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
-						local_pos.ref_alt = baro_alt0;
-						local_pos.ref_timestamp = hrt_absolute_time();
+						baro_offset /= (float) baro_init_cnt;
+						warnx("baro offs: %.2f", (double)baro_offset);
+						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
-						local_pos.z_global = true;
 					}
 				}
 			}
 		}
 	}
 
-	bool ref_xy_inited = false;
-	hrt_abstime ref_xy_init_start = 0;
-	const hrt_abstime ref_xy_init_delay = 5000000;	// wait for 5s after 3D fix
-
-	hrt_abstime t_prev = 0;
-
-	uint16_t accel_updates = 0;
-	uint16_t baro_updates = 0;
-	uint16_t gps_updates = 0;
-	uint16_t attitude_updates = 0;
-	uint16_t flow_updates = 0;
-
-	hrt_abstime updates_counter_start = hrt_absolute_time();
-	hrt_abstime pub_last = hrt_absolute_time();
-
-	/* acceleration in NED frame */
-	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
-
-	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
-	float accel_corr[] = { 0.0f, 0.0f, 0.0f };	// N E D
-	float accel_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
-	float baro_corr = 0.0f;		// D
-	float gps_corr[2][2] = {
-		{ 0.0f, 0.0f },		// N (pos, vel)
-		{ 0.0f, 0.0f },		// E (pos, vel)
-	};
-	float sonar_corr = 0.0f;
-	float sonar_corr_filtered = 0.0f;
-	float flow_corr[] = { 0.0f, 0.0f };	// X, Y
-
-	float sonar_prev = 0.0f;
-	hrt_abstime sonar_time = 0;
-
 	/* main loop */
-	struct pollfd fds[7] = {
-		{ .fd = parameter_update_sub, .events = POLLIN },
-		{ .fd = actuator_sub, .events = POLLIN },
-		{ .fd = armed_sub, .events = POLLIN },
+	struct pollfd fds[1] = {
 		{ .fd = vehicle_attitude_sub, .events = POLLIN },
-		{ .fd = sensor_combined_sub, .events = POLLIN },
-		{ .fd = optical_flow_sub, .events = POLLIN },
-		{ .fd = vehicle_gps_position_sub, .events = POLLIN }
 	};
 
-	if (!thread_should_exit) {
-		warnx("main loop started.");
-	}
-
 	while (!thread_should_exit) {
-		int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
 		hrt_abstime t = hrt_absolute_time();
 
 		if (ret < 0) {
@@ -324,237 +398,548 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			continue;
 
 		} else if (ret > 0) {
+			/* act on attitude updates */
+
+			/* vehicle attitude */
+			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+			attitude_updates++;
+
+			bool updated;
+
 			/* parameter update */
-			if (fds[0].revents & POLLIN) {
-				/* read from param to clear updated flag */
+			orb_check(parameter_update_sub, &updated);
+
+			if (updated) {
 				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), parameter_update_sub,
-					 &update);
-				/* update parameters */
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
 				parameters_update(&pos_inav_param_handles, &params);
 			}
 
 			/* actuator */
-			if (fds[1].revents & POLLIN) {
+			orb_check(actuator_sub, &updated);
+
+			if (updated) {
 				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
 			}
 
 			/* armed */
-			if (fds[2].revents & POLLIN) {
-				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-			}
+			orb_check(armed_sub, &updated);
 
-			/* vehicle attitude */
-			if (fds[3].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-				attitude_updates++;
+			if (updated) {
+				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 			}
 
 			/* sensor combined */
-			if (fds[4].revents & POLLIN) {
+			orb_check(sensor_combined_sub, &updated);
+
+			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (sensor.accelerometer_counter != accel_counter) {
+				if (sensor.accelerometer_timestamp != accel_timestamp) {
 					if (att.R_valid) {
-						/* correct accel bias, now only for Z */
-						sensor.accelerometer_m_s2[2] -= accel_bias[2];
+						/* correct accel bias */
+						sensor.accelerometer_m_s2[0] -= acc_bias[0];
+						sensor.accelerometer_m_s2[1] -= acc_bias[1];
+						sensor.accelerometer_m_s2[2] -= acc_bias[2];
 
 						/* transform acceleration vector from body frame to NED frame */
 						for (int i = 0; i < 3; i++) {
-							accel_NED[i] = 0.0f;
+							acc[i] = 0.0f;
 
 							for (int j = 0; j < 3; j++) {
-								accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+								acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
 							}
 						}
 
-						accel_corr[0] = accel_NED[0] - x_est[2];
-						accel_corr[1] = accel_NED[1] - y_est[2];
-						accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
+						acc[2] += CONSTANTS_ONE_G;
 
 					} else {
-						memset(accel_corr, 0, sizeof(accel_corr));
+						memset(acc, 0, sizeof(acc));
 					}
 
-					accel_counter = sensor.accelerometer_counter;
+					accel_timestamp = sensor.accelerometer_timestamp;
 					accel_updates++;
 				}
 
-				if (sensor.baro_counter != baro_counter) {
-					baro_corr = - sensor.baro_alt_meter - z_est[0];
-					baro_counter = sensor.baro_counter;
+				if (sensor.baro_timestamp != baro_timestamp) {
+					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
+					baro_timestamp = sensor.baro_timestamp;
 					baro_updates++;
 				}
 			}
 
 			/* optical flow */
-			if (fds[5].revents & POLLIN) {
+			orb_check(optical_flow_sub, &updated);
+
+			if (updated) {
 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
 
-				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
-					if (flow.ground_distance_m != sonar_prev) {
-						sonar_time = t;
-						sonar_prev = flow.ground_distance_m;
-						sonar_corr = -flow.ground_distance_m - z_est[0];
-						sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
-
-						if (fabsf(sonar_corr) > params.sonar_err) {
-							// correction is too large: spike or new ground level?
-							if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
-								// spike detected, ignore
-								sonar_corr = 0.0f;
-
-							} else {
-								// new ground level
-								baro_alt0 += sonar_corr;
-								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
-								local_pos.ref_alt = baro_alt0;
-								local_pos.ref_timestamp = hrt_absolute_time();
-								z_est[0] += sonar_corr;
-								sonar_corr = 0.0f;
-								sonar_corr_filtered = 0.0f;
-							}
+				/* calculate time from previous update */
+				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
+				flow_prev = flow.flow_timestamp;
+
+				if ((flow.ground_distance_m > 0.31f) &&
+					(flow.ground_distance_m < 4.0f) &&
+					(att.R[2][2] > 0.7f) &&
+					(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
+
+					sonar_time = t;
+					sonar_prev = flow.ground_distance_m;
+					corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
+					corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;
+
+					if (fabsf(corr_sonar) > params.sonar_err) {
+						/* correction is too large: spike or new ground level? */
+						if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
+							/* spike detected, ignore */
+							corr_sonar = 0.0f;
+							sonar_valid = false;
+
+						} else {
+							/* new ground level */
+							surface_offset -= corr_sonar;
+							surface_offset_rate = 0.0f;
+							corr_sonar = 0.0f;
+							corr_sonar_filtered = 0.0f;
+							sonar_valid_time = t;
+							sonar_valid = true;
+							local_pos.surface_bottom_timestamp = t;
+							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
+						}
+
+					} else {
+						/* correction is ok, use it */
+						sonar_valid_time = t;
+						sonar_valid = true;
+					}
+				}
+
+				float flow_q = flow.quality / 255.0f;
+				float dist_bottom = - z_est[0] - surface_offset;
+
+				if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
+					/* distance to surface */
+					float flow_dist = dist_bottom / att.R[2][2];
+					/* check if flow if too large for accurate measurements */
+					/* calculate estimated velocity in body frame */
+					float body_v_est[2] = { 0.0f, 0.0f };
+
+					for (int i = 0; i < 2; i++) {
+						body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
+					}
+
+					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
+					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
+							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
+
+					/* convert raw flow to angular flow (rad/s) */
+					float flow_ang[2];
+					flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
+					flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
+					/* flow measurements vector */
+					float flow_m[3];
+					flow_m[0] = -flow_ang[0] * flow_dist;
+					flow_m[1] = -flow_ang[1] * flow_dist;
+					flow_m[2] = z_est[1];
+					/* velocity in NED */
+					float flow_v[2] = { 0.0f, 0.0f };
+
+					/* project measurements vector to NED basis, skip Z component */
+					for (int i = 0; i < 2; i++) {
+						for (int j = 0; j < 3; j++) {
+							flow_v[i] += att.R[i][j] * flow_m[j];
 						}
 					}
 
+					/* velocity correction */
+					corr_flow[0] = flow_v[0] - x_est[1];
+					corr_flow[1] = flow_v[1] - y_est[1];
+					/* adjust correction weight */
+					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
+					w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
+
+					/* if flow is not accurate, reduce weight for it */
+					// TODO make this more fuzzy
+					if (!flow_accurate) {
+						w_flow *= 0.05f;
+					}
+
+					/* under ideal conditions, on 1m distance assume EPH = 10cm */
+					eph_flow = 0.1f / w_flow;
+
+					flow_valid = true;
+
 				} else {
-					sonar_corr = 0.0f;
+					w_flow = 0.0f;
+					flow_valid = false;
 				}
 
 				flow_updates++;
 			}
 
+			/* home position */
+			orb_check(home_position_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(home_position), home_position_sub, &home);
+
+				if (home.timestamp != home_timestamp) {
+					home_timestamp = home.timestamp;
+
+					double est_lat, est_lon;
+					float est_alt;
+
+					if (ref_inited) {
+						/* calculate current estimated position in global frame */
+						est_alt = local_pos.ref_alt - local_pos.z;
+						map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
+					}
+
+					/* update reference */
+					map_projection_init(&ref, home.lat, home.lon);
+
+					/* update baro offset */
+					baro_offset += home.alt - local_pos.ref_alt;
+
+					local_pos.ref_lat = home.lat;
+					local_pos.ref_lon = home.lon;
+					local_pos.ref_alt = home.alt;
+					local_pos.ref_timestamp = home.timestamp;
+
+					if (ref_inited) {
+						/* reproject position estimate with new reference */
+						map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
+						z_est[0] = -(est_alt - local_pos.ref_alt);
+					}
+
+					ref_inited = true;
+				}
+			}
+
 			/* vehicle GPS position */
-			if (fds[6].revents & POLLIN) {
+			orb_check(vehicle_gps_position_sub, &updated);
+
+			if (updated) {
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
-				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
+				bool reset_est = false;
+
+				/* hysteresis for GPS quality */
+				if (gps_valid) {
+					if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
+						gps_valid = false;
+						mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
+					}
+
+				} else {
+					if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
+						gps_valid = true;
+						reset_est = true;
+						mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
+					}
+				}
+
+				if (gps_valid) {
+					double lat = gps.lat * 1e-7;
+					double lon = gps.lon * 1e-7;
+					float alt = gps.alt * 1e-3;
+
 					/* initialize reference position if needed */
-					if (!ref_xy_inited) {
-						/* require EPH < 10m */
-						if (gps.eph_m < 10.0f) {
-							if (ref_xy_init_start == 0) {
-								ref_xy_init_start = t;
-
-							} else if (t > ref_xy_init_start + ref_xy_init_delay) {
-								ref_xy_inited = true;
-								/* reference GPS position */
-								double lat = gps.lat * 1e-7;
-								double lon = gps.lon * 1e-7;
-
-								local_pos.ref_lat = gps.lat;
-								local_pos.ref_lon = gps.lon;
-								local_pos.ref_timestamp = t;
-
-								/* initialize projection */
-								map_projection_init(lat, lon);
-								warnx("init GPS: lat = %.10f,  lon = %.10f", lat, lon);
-								mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
-							}
-						} else {
-							ref_xy_init_start = 0;
+					if (!ref_inited) {
+						if (ref_init_start == 0) {
+							ref_init_start = t;
+
+						} else if (t > ref_init_start + ref_init_delay) {
+							ref_inited = true;
+
+							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
+							x_est[0] = 0.0f;
+							x_est[1] = gps.vel_n_m_s;
+							y_est[0] = 0.0f;
+							y_est[1] = gps.vel_e_m_s;
+
+							local_pos.ref_lat = lat;
+							local_pos.ref_lon = lon;
+							local_pos.ref_alt = alt + z_est[0];
+							local_pos.ref_timestamp = t;
+
+							/* initialize projection */
+							map_projection_init(&ref, lat, lon);
+							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
+							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
 						}
 					}
 
-					if (ref_xy_inited) {
+					if (ref_inited) {
 						/* project GPS lat lon to plane */
 						float gps_proj[2];
-						map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+						map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
+
+						/* reset position estimate when GPS becomes good */
+						if (reset_est) {
+							x_est[0] = gps_proj[0];
+							x_est[1] = gps.vel_n_m_s;
+							y_est[0] = gps_proj[1];
+							y_est[1] = gps.vel_e_m_s;
+						}
+
+						/* calculate index of estimated values in buffer */
+						int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
+						if (est_i < 0) {
+							est_i += EST_BUF_SIZE;
+						}
+
 						/* calculate correction for position */
-						gps_corr[0][0] = gps_proj[0] - x_est[0];
-						gps_corr[1][0] = gps_proj[1] - y_est[0];
+						corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
+						corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
+						corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
 
 						/* calculate correction for velocity */
 						if (gps.vel_ned_valid) {
-							gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
-							gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+							corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
+							corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
+							corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
 
 						} else {
-							gps_corr[0][1] = 0.0f;
-							gps_corr[1][1] = 0.0f;
+							corr_gps[0][1] = 0.0f;
+							corr_gps[1][1] = 0.0f;
+							corr_gps[2][1] = 0.0f;
 						}
+
+						/* save rotation matrix at this moment */
+						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
+
+						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 {
 					/* no GPS lock */
-					memset(gps_corr, 0, sizeof(gps_corr));
-					ref_xy_init_start = 0;
+					memset(corr_gps, 0, sizeof(corr_gps));
+					ref_init_start = 0;
 				}
 
 				gps_updates++;
 			}
 		}
 
-		/* end of poll return value check */
+		/* check for timeout on FLOW topic */
+		if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) {
+			flow_valid = false;
+			sonar_valid = false;
+			warnx("FLOW timeout");
+			mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
+		}
+
+		/* check for timeout on GPS topic */
+		if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) {
+			gps_valid = false;
+			warnx("GPS timeout");
+			mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
+		}
+
+		/* check for sonar measurement timeout */
+		if (sonar_valid && t > sonar_time + sonar_timeout) {
+			corr_sonar = 0.0f;
+			sonar_valid = false;
+		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
+		dt = fmaxf(fminf(0.02, dt), 0.002);		// constrain dt from 2 to 20 ms
 		t_prev = t;
 
-		/* reset ground level on arm */
-		if (armed.armed && !flag_armed) {
-			baro_alt0 -= z_est[0];
-			z_est[0] = 0.0f;
-			local_pos.ref_alt = baro_alt0;
-			local_pos.ref_timestamp = hrt_absolute_time();
-			mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
+		/* increase EPH/EPV on each step */
+		if (eph < max_eph_epv) {
+			eph *= 1.0f + dt;
+		}
+		if (epv < max_eph_epv) {
+			epv += 0.005f * dt;	// add 1m to EPV each 200s (baro drift)
+		}
+
+		/* use GPS if it's valid and reference position initialized */
+		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
+		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
+		/* use flow if it's valid and (accurate or no GPS available) */
+		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
+
+		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
+
+		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
+
+		if (dist_bottom_valid) {
+			/* surface distance prediction */
+			surface_offset += surface_offset_rate * dt;
+
+			/* surface distance correction */
+			if (sonar_valid) {
+				surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
+				surface_offset -= corr_sonar * params.w_z_sonar * dt;
+			}
+		}
+
+		float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
+		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
+		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
+
+		/* reduce GPS weight if optical flow is good */
+		if (use_flow && flow_accurate) {
+			w_xy_gps_p *= params.w_gps_flow;
+			w_xy_gps_v *= params.w_gps_flow;
+		}
+
+		/* baro offset correction */
+		if (use_gps_z) {
+			float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
+			baro_offset += offs_corr;
+			corr_baro += offs_corr;
+		}
+
+		/* accelerometer bias correction for GPS (use buffered rotation matrix) */
+		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
+
+		if (use_gps_xy) {
+			accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
+			accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
+			accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
+			accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
+		}
+
+		if (use_gps_z) {
+			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
+		}
+
+		/* transform error vector from NED frame to body frame */
+		for (int i = 0; i < 3; i++) {
+			float c = 0.0f;
+
+			for (int j = 0; j < 3; j++) {
+				c += R_gps[j][i] * accel_bias_corr[j];
+			}
+
+			if (isfinite(c)) {
+				acc_bias[i] += c * params.w_acc_bias * dt;
+			}
+		}
+
+		/* accelerometer bias correction for flow and baro (assume that there is no delay) */
+		accel_bias_corr[0] = 0.0f;
+		accel_bias_corr[1] = 0.0f;
+		accel_bias_corr[2] = 0.0f;
+
+		if (use_flow) {
+			accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
+			accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
 		}
 
-		/* accel bias correction, now only for Z
-		 * not strictly correct, but stable and works */
-		accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
+		accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
+
+		/* transform error vector from NED frame to body frame */
+		for (int i = 0; i < 3; i++) {
+			float c = 0.0f;
+
+			for (int j = 0; j < 3; j++) {
+				c += att.R[j][i] * accel_bias_corr[j];
+			}
+
+			if (isfinite(c)) {
+				acc_bias[i] += c * params.w_acc_bias * dt;
+			}
+		}
 
 		/* inertial filter prediction for altitude */
-		inertial_filter_predict(dt, z_est);
+		inertial_filter_predict(dt, z_est, acc[2]);
+
+		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
+			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+			memcpy(z_est, z_est_prev, sizeof(z_est));
+		}
 
 		/* inertial filter correction for altitude */
-		baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
-		inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
-		inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
-		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
+		inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
 
-		bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
-		bool flow_valid = false;	// TODO implement opt flow
+		if (use_gps_z) {
+			epv = fminf(epv, gps.epv);
+
+			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
+		}
 
-		/* try to estimate xy even if no absolute position source available,
-		 * if using optical flow velocity will be correct in this case */
-		bool can_estimate_xy = gps_valid || flow_valid;
+		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
+			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+			memcpy(z_est, z_est_prev, sizeof(z_est));
+			memset(corr_gps, 0, sizeof(corr_gps));
+			corr_baro = 0;
+
+		} else {
+			memcpy(z_est_prev, z_est, sizeof(z_est));
+		}
 
 		if (can_estimate_xy) {
 			/* inertial filter prediction for position */
-			inertial_filter_predict(dt, x_est);
-			inertial_filter_predict(dt, y_est);
+			inertial_filter_predict(dt, x_est, acc[0]);
+			inertial_filter_predict(dt, y_est, acc[1]);
+
+			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
+				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+				memcpy(x_est, x_est_prev, sizeof(x_est));
+				memcpy(y_est, y_est_prev, sizeof(y_est));
+			}
 
 			/* inertial filter correction for position */
-			inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
-			inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
+			if (use_flow) {
+				eph = fminf(eph, eph_flow);
+
+				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
+				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
+			}
+
+			if (use_gps_xy) {
+				eph = fminf(eph, gps.eph);
 
-			if (gps_valid) {
-				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
-				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
+				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
+				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
 
-				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
-					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
-					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
+				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
+					inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
+					inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
 				}
 			}
+
+			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
+				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+				memcpy(x_est, x_est_prev, sizeof(x_est));
+				memcpy(y_est, y_est_prev, sizeof(y_est));
+				memset(corr_gps, 0, sizeof(corr_gps));
+				memset(corr_flow, 0, sizeof(corr_flow));
+
+			} else {
+				memcpy(x_est_prev, x_est, sizeof(x_est));
+				memcpy(y_est_prev, y_est, sizeof(y_est));
+			}
+		} else {
+			/* gradually reset xy velocity estimates */
+			inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
+			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
 		}
 
 		/* detect land */
-		alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
-		float alt_disp = z_est[0] - alt_avg;
-		alt_disp = alt_disp * alt_disp;
+		alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
+		float alt_disp2 = - z_est[0] - alt_avg;
+		alt_disp2 = alt_disp2 * alt_disp2;
 		float land_disp2 = params.land_disp * params.land_disp;
 		/* get actual thrust output */
 		float thrust = armed.armed ? actuator.control[3] : 0.0f;
 
 		if (landed) {
-			if (alt_disp > land_disp2 && thrust > params.land_thr) {
+			if (alt_disp2 > land_disp2 || thrust > params.land_thr) {
 				landed = false;
 				landed_time = 0;
 			}
+			/* reset xy velocity estimates when landed */
+			x_est[1] = 0.0f;
+			y_est[1] = 0.0f;
 
 		} else {
-			if (alt_disp < land_disp2 && thrust < params.land_thr) {
+			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
 				if (landed_time == 0) {
 					landed_time = t;    // land detected first time
 
@@ -576,11 +961,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				float updates_dt = (t - updates_counter_start) * 0.000001f;
 				warnx(
 					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
-					accel_updates / updates_dt,
-					baro_updates / updates_dt,
-					gps_updates / updates_dt,
-					attitude_updates / updates_dt,
-					flow_updates / updates_dt);
+					(double)(accel_updates / updates_dt),
+					(double)(baro_updates / updates_dt),
+					(double)(gps_updates / updates_dt),
+					(double)(attitude_updates / updates_dt),
+					(double)(flow_updates / updates_dt));
 				updates_counter_start = t;
 				accel_updates = 0;
 				baro_updates = 0;
@@ -590,13 +975,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (t > pub_last + pub_interval) {
+		if (t > pub_last + PUB_INTERVAL) {
 			pub_last = t;
+
+			/* push current estimate to buffer */
+			est_buf[buf_ptr][0][0] = x_est[0];
+			est_buf[buf_ptr][0][1] = x_est[1];
+			est_buf[buf_ptr][1][0] = y_est[0];
+			est_buf[buf_ptr][1][1] = y_est[1];
+			est_buf[buf_ptr][2][0] = z_est[0];
+			est_buf[buf_ptr][2][1] = z_est[1];
+
+			/* push current rotation matrix to buffer */
+			memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
+
+			buf_ptr++;
+			if (buf_ptr >= EST_BUF_SIZE) {
+				buf_ptr = 0;
+			}
+
 			/* publish local position */
-			local_pos.timestamp = t;
-			local_pos.xy_valid = can_estimate_xy && gps_valid;
+			local_pos.xy_valid = can_estimate_xy;
 			local_pos.v_xy_valid = can_estimate_xy;
-			local_pos.xy_global = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
+			local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
+			local_pos.z_global = local_pos.z_valid && use_gps_z;
 			local_pos.x = x_est[0];
 			local_pos.vx = x_est[1];
 			local_pos.y = y_est[0];
@@ -605,48 +1007,52 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.vz = z_est[1];
 			local_pos.landed = landed;
 			local_pos.yaw = att.yaw;
+			local_pos.dist_bottom_valid = dist_bottom_valid;
+			local_pos.eph = eph;
+			local_pos.epv = epv;
+
+			if (local_pos.dist_bottom_valid) {
+				local_pos.dist_bottom = -z_est[0] - surface_offset;
+				local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
+			}
+
+			local_pos.timestamp = t;
 
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
-			/* publish global position */
-			global_pos.valid = local_pos.xy_global;
+			if (local_pos.xy_global && local_pos.z_global) {
+				/* publish global position */
+				global_pos.timestamp = t;
+				global_pos.time_gps_usec = gps.time_gps_usec;
 
-			if (local_pos.xy_global) {
 				double est_lat, est_lon;
-				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
-				global_pos.lat = (int32_t)(est_lat * 1e7);
-				global_pos.lon = (int32_t)(est_lon * 1e7);
-				global_pos.time_gps_usec = gps.time_gps_usec;
-			}
+				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
 
-			/* set valid values even if position is not valid */
-			if (local_pos.v_xy_valid) {
-				global_pos.vx = local_pos.vx;
-				global_pos.vy = local_pos.vy;
-			}
+				global_pos.lat = est_lat;
+				global_pos.lon = est_lon;
+				global_pos.alt = local_pos.ref_alt - local_pos.z;
 
-			if (local_pos.z_valid) {
-				global_pos.relative_alt = -local_pos.z;
-			}
+				global_pos.vel_n = local_pos.vx;
+				global_pos.vel_e = local_pos.vy;
+				global_pos.vel_d = local_pos.vz;
 
-			if (local_pos.z_global) {
-				global_pos.alt = local_pos.ref_alt - local_pos.z;
-			}
+				global_pos.yaw = local_pos.yaw;
 
-			if (local_pos.v_z_valid) {
-				global_pos.vz = local_pos.vz;
-			}
-			global_pos.yaw = local_pos.yaw;
+				global_pos.eph = eph;
+				global_pos.epv = epv;
 
-			global_pos.timestamp = t;
+				if (vehicle_global_position_pub < 0) {
+					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
 
-			orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
+				} else {
+					orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
+				}
+			}
 		}
-		flag_armed = armed.armed;
 	}
 
-	warnx("exiting.");
-	mavlink_log_info(mavlink_fd, "[inav] exiting");
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[inav] stopped");
 	thread_running = false;
 	return 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 4f9ddd009..0581f8236 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -40,57 +40,66 @@
 
 #include "position_estimator_inav_params.h"
 
-PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
-PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
-PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
-PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
-PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
-PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
-PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
-PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
+PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
+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);
+PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
-	h->w_alt_baro = param_find("INAV_W_ALT_BARO");
-	h->w_alt_acc = param_find("INAV_W_ALT_ACC");
-	h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
-	h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
-	h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
-	h->w_pos_acc = param_find("INAV_W_POS_ACC");
-	h->w_pos_flow = param_find("INAV_W_POS_FLOW");
+	h->w_z_baro = param_find("INAV_W_Z_BARO");
+	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
+	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
+	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
+	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
+	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
+	h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
+	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
 	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
 	h->flow_k = param_find("INAV_FLOW_K");
+	h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
 	h->sonar_filt = param_find("INAV_SONAR_FILT");
 	h->sonar_err = param_find("INAV_SONAR_ERR");
 	h->land_t = param_find("INAV_LAND_T");
 	h->land_disp = param_find("INAV_LAND_DISP");
 	h->land_thr = param_find("INAV_LAND_THR");
+	h->delay_gps = param_find("INAV_DELAY_GPS");
 
 	return OK;
 }
 
 int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
 {
-	param_get(h->w_alt_baro, &(p->w_alt_baro));
-	param_get(h->w_alt_acc, &(p->w_alt_acc));
-	param_get(h->w_alt_sonar, &(p->w_alt_sonar));
-	param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
-	param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
-	param_get(h->w_pos_acc, &(p->w_pos_acc));
-	param_get(h->w_pos_flow, &(p->w_pos_flow));
+	param_get(h->w_z_baro, &(p->w_z_baro));
+	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
+	param_get(h->w_z_sonar, &(p->w_z_sonar));
+	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
+	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
+	param_get(h->w_xy_flow, &(p->w_xy_flow));
+	param_get(h->w_xy_res_v, &(p->w_xy_res_v));
+	param_get(h->w_gps_flow, &(p->w_gps_flow));
 	param_get(h->w_acc_bias, &(p->w_acc_bias));
 	param_get(h->flow_k, &(p->flow_k));
+	param_get(h->flow_q_min, &(p->flow_q_min));
 	param_get(h->sonar_filt, &(p->sonar_filt));
 	param_get(h->sonar_err, &(p->sonar_err));
 	param_get(h->land_t, &(p->land_t));
 	param_get(h->land_disp, &(p->land_disp));
 	param_get(h->land_thr, &(p->land_thr));
+	param_get(h->delay_gps, &(p->delay_gps));
 
 	return OK;
 }
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index 61570aea7..423f0d879 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -41,37 +41,43 @@
 #include <systemlib/param/param.h>
 
 struct position_estimator_inav_params {
-	float w_alt_baro;
-	float w_alt_acc;
-	float w_alt_sonar;
-	float w_pos_gps_p;
-	float w_pos_gps_v;
-	float w_pos_acc;
-	float w_pos_flow;
+	float w_z_baro;
+	float w_z_gps_p;
+	float w_z_sonar;
+	float w_xy_gps_p;
+	float w_xy_gps_v;
+	float w_xy_flow;
+	float w_xy_res_v;
+	float w_gps_flow;
 	float w_acc_bias;
 	float flow_k;
+	float flow_q_min;
 	float sonar_filt;
 	float sonar_err;
 	float land_t;
 	float land_disp;
 	float land_thr;
+	float delay_gps;
 };
 
 struct position_estimator_inav_param_handles {
-	param_t w_alt_baro;
-	param_t w_alt_acc;
-	param_t w_alt_sonar;
-	param_t w_pos_gps_p;
-	param_t w_pos_gps_v;
-	param_t w_pos_acc;
-	param_t w_pos_flow;
+	param_t w_z_baro;
+	param_t w_z_gps_p;
+	param_t w_z_sonar;
+	param_t w_xy_gps_p;
+	param_t w_xy_gps_v;
+	param_t w_xy_flow;
+	param_t w_xy_res_v;
+	param_t w_gps_flow;
 	param_t w_acc_bias;
 	param_t flow_k;
+	param_t flow_q_min;
 	param_t sonar_filt;
 	param_t sonar_err;
 	param_t land_t;
 	param_t land_disp;
 	param_t land_thr;
+	param_t delay_gps;
 };
 
 /**
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c
deleted file mode 100755
index 977565b8e..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * kalman_dlqe1.c
- *
- * Code generation for function 'kalman_dlqe1'
- *
- * C source code generated on: Wed Feb 13 20:34:32 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe1.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3],
-                  const real32_T x_aposteriori_k[3], real32_T z, real32_T
-                  x_aposteriori[3])
-{
-	printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z);
-	printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2]));
-  real32_T y;
-  int32_T i0;
-  real32_T b_y[3];
-  int32_T i1;
-  real32_T f0;
-  y = 0.0F;
-  for (i0 = 0; i0 < 3; i0++) {
-    b_y[i0] = 0.0F;
-    for (i1 = 0; i1 < 3; i1++) {
-      b_y[i0] += C[i1] * A[i1 + 3 * i0];
-    }
-
-    y += b_y[i0] * x_aposteriori_k[i0];
-  }
-
-  y = z - y;
-  for (i0 = 0; i0 < 3; i0++) {
-    f0 = 0.0F;
-    for (i1 = 0; i1 < 3; i1++) {
-      f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
-    }
-
-    x_aposteriori[i0] = f0 + K[i0] * y;
-  }
-  //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2]));
-}
-
-/* End of code generation (kalman_dlqe1.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h
deleted file mode 100755
index 2f5330563..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * kalman_dlqe1.h
- *
- * Code generation for function 'kalman_dlqe1'
- *
- * C source code generated on: Wed Feb 13 20:34:32 2013
- *
- */
-
-#ifndef __KALMAN_DLQE1_H__
-#define __KALMAN_DLQE1_H__
-/* Include files */
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "kalman_dlqe1_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
-#endif
-/* End of code generation (kalman_dlqe1.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
deleted file mode 100755
index 6627f76e9..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * kalman_dlqe1_initialize.c
- *
- * Code generation for function 'kalman_dlqe1_initialize'
- *
- * C source code generated on: Wed Feb 13 20:34:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe1.h"
-#include "kalman_dlqe1_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe1_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (kalman_dlqe1_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
deleted file mode 100755
index a77eb5712..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * kalman_dlqe1_initialize.h
- *
- * Code generation for function 'kalman_dlqe1_initialize'
- *
- * C source code generated on: Wed Feb 13 20:34:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE1_INITIALIZE_H__
-#define __KALMAN_DLQE1_INITIALIZE_H__
-/* Include files */
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "kalman_dlqe1_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe1_initialize(void);
-#endif
-/* End of code generation (kalman_dlqe1_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
deleted file mode 100755
index a65536f79..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * kalman_dlqe1_terminate.c
- *
- * Code generation for function 'kalman_dlqe1_terminate'
- *
- * C source code generated on: Wed Feb 13 20:34:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe1.h"
-#include "kalman_dlqe1_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe1_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (kalman_dlqe1_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
deleted file mode 100755
index 100c7f76c..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * kalman_dlqe1_terminate.h
- *
- * Code generation for function 'kalman_dlqe1_terminate'
- *
- * C source code generated on: Wed Feb 13 20:34:32 2013
- *
- */
-
-#ifndef __KALMAN_DLQE1_TERMINATE_H__
-#define __KALMAN_DLQE1_TERMINATE_H__
-/* Include files */
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "kalman_dlqe1_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe1_terminate(void);
-#endif
-/* End of code generation (kalman_dlqe1_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h
deleted file mode 100755
index d4b2c2d61..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * kalman_dlqe1_types.h
- *
- * Code generation for function 'kalman_dlqe1'
- *
- * C source code generated on: Wed Feb 13 20:34:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE1_TYPES_H__
-#define __KALMAN_DLQE1_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (kalman_dlqe1_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c
deleted file mode 100755
index 11b999064..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c
+++ /dev/null
@@ -1,119 +0,0 @@
-/*
- * kalman_dlqe2.c
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe2.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static real32_T rt_powf_snf(real32_T u0, real32_T u1);
-
-/* Function Definitions */
-static real32_T rt_powf_snf(real32_T u0, real32_T u1)
-{
-  real32_T y;
-  real32_T f1;
-  real32_T f2;
-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
-    y = ((real32_T)rtNaN);
-  } else {
-    f1 = (real32_T)fabs(u0);
-    f2 = (real32_T)fabs(u1);
-    if (rtIsInfF(u1)) {
-      if (f1 == 1.0F) {
-        y = ((real32_T)rtNaN);
-      } else if (f1 > 1.0F) {
-        if (u1 > 0.0F) {
-          y = ((real32_T)rtInf);
-        } else {
-          y = 0.0F;
-        }
-      } else if (u1 > 0.0F) {
-        y = 0.0F;
-      } else {
-        y = ((real32_T)rtInf);
-      }
-    } else if (f2 == 0.0F) {
-      y = 1.0F;
-    } else if (f2 == 1.0F) {
-      if (u1 > 0.0F) {
-        y = u0;
-      } else {
-        y = 1.0F / u0;
-      }
-    } else if (u1 == 2.0F) {
-      y = u0 * u0;
-    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
-      y = (real32_T)sqrt(u0);
-    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
-      y = ((real32_T)rtNaN);
-    } else {
-      y = (real32_T)pow(u0, u1);
-    }
-  }
-
-  return y;
-}
-
-void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
-                  real32_T x_aposteriori_k[3], real32_T z, real32_T
-                  x_aposteriori[3])
-{
-	//printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3));
-	//printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3));
-  real32_T A[9];
-  real32_T y;
-  int32_T i0;
-  static const int8_T iv0[3] = { 0, 0, 1 };
-
-  real32_T b_k1[3];
-  int32_T i1;
-  static const int8_T iv1[3] = { 1, 0, 0 };
-
-  real32_T f0;
-  A[0] = 1.0F;
-  A[3] = dt;
-  A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
-  A[1] = 0.0F;
-  A[4] = 1.0F;
-  A[7] = dt;
-  y = 0.0F;
-  for (i0 = 0; i0 < 3; i0++) {
-    A[2 + 3 * i0] = (real32_T)iv0[i0];
-    b_k1[i0] = 0.0F;
-    for (i1 = 0; i1 < 3; i1++) {
-      b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
-    }
-
-    y += b_k1[i0] * x_aposteriori_k[i0];
-  }
-
-  y = z - y;
-  b_k1[0] = k1;
-  b_k1[1] = k2;
-  b_k1[2] = k3;
-  for (i0 = 0; i0 < 3; i0++) {
-    f0 = 0.0F;
-    for (i1 = 0; i1 < 3; i1++) {
-      f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
-    }
-
-    x_aposteriori[i0] = f0 + b_k1[i0] * y;
-  }
-}
-
-/* End of code generation (kalman_dlqe2.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h
deleted file mode 100755
index 30170ae22..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * kalman_dlqe2.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __KALMAN_DLQE2_H__
-#define __KALMAN_DLQE2_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe2_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
-#endif
-/* End of code generation (kalman_dlqe2.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
deleted file mode 100755
index de5a1d8aa..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * kalman_dlqe2_initialize.c
- *
- * Code generation for function 'kalman_dlqe2_initialize'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe2.h"
-#include "kalman_dlqe2_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe2_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (kalman_dlqe2_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
deleted file mode 100755
index 3d507ff31..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * kalman_dlqe2_initialize.h
- *
- * Code generation for function 'kalman_dlqe2_initialize'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-#ifndef __KALMAN_DLQE2_INITIALIZE_H__
-#define __KALMAN_DLQE2_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe2_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe2_initialize(void);
-#endif
-/* End of code generation (kalman_dlqe2_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
deleted file mode 100755
index 0757c878c..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * kalman_dlqe2_terminate.c
- *
- * Code generation for function 'kalman_dlqe2_terminate'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe2.h"
-#include "kalman_dlqe2_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe2_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (kalman_dlqe2_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
deleted file mode 100755
index 23995020b..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * kalman_dlqe2_terminate.h
- *
- * Code generation for function 'kalman_dlqe2_terminate'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-#ifndef __KALMAN_DLQE2_TERMINATE_H__
-#define __KALMAN_DLQE2_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe2_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe2_terminate(void);
-#endif
-/* End of code generation (kalman_dlqe2_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h
deleted file mode 100755
index f7a04d908..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * kalman_dlqe2_types.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-#ifndef __KALMAN_DLQE2_TYPES_H__
-#define __KALMAN_DLQE2_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (kalman_dlqe2_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c
deleted file mode 100755
index 9efe2ea7a..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c
+++ /dev/null
@@ -1,137 +0,0 @@
-/*

- * kalman_dlqe3.c

- *

- * Code generation for function 'kalman_dlqe3'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "randn.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-static real32_T rt_powf_snf(real32_T u0, real32_T u1);

-

-/* Function Definitions */

-static real32_T rt_powf_snf(real32_T u0, real32_T u1)

-{

-  real32_T y;

-  real32_T f1;

-  real32_T f2;

-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

-    y = ((real32_T)rtNaN);

-  } else {

-    f1 = (real32_T)fabs(u0);

-    f2 = (real32_T)fabs(u1);

-    if (rtIsInfF(u1)) {

-      if (f1 == 1.0F) {

-        y = ((real32_T)rtNaN);

-      } else if (f1 > 1.0F) {

-        if (u1 > 0.0F) {

-          y = ((real32_T)rtInf);

-        } else {

-          y = 0.0F;

-        }

-      } else if (u1 > 0.0F) {

-        y = 0.0F;

-      } else {

-        y = ((real32_T)rtInf);

-      }

-    } else if (f2 == 0.0F) {

-      y = 1.0F;

-    } else if (f2 == 1.0F) {

-      if (u1 > 0.0F) {

-        y = u0;

-      } else {

-        y = 1.0F / u0;

-      }

-    } else if (u1 == 2.0F) {

-      y = u0 * u0;

-    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {

-      y = (real32_T)sqrt(u0);

-    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {

-      y = ((real32_T)rtNaN);

-    } else {

-      y = (real32_T)pow(u0, u1);

-    }

-  }

-

-  return y;

-}

-

-void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const

-                  real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,

-                  real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])

-{

-  real32_T A[9];

-  int32_T i0;

-  static const int8_T iv0[3] = { 0, 0, 1 };

-

-  real_T b;

-  real32_T y;

-  real32_T b_y[3];

-  int32_T i1;

-  static const int8_T iv1[3] = { 1, 0, 0 };

-

-  real32_T b_k1[3];

-  real32_T f0;

-  A[0] = 1.0F;

-  A[3] = dt;

-  A[6] = 0.5F * rt_powf_snf(dt, 2.0F);

-  A[1] = 0.0F;

-  A[4] = 1.0F;

-  A[7] = dt;

-  for (i0 = 0; i0 < 3; i0++) {

-    A[2 + 3 * i0] = (real32_T)iv0[i0];

-  }

-

-  if (addNoise == 1.0F) {

-    b = randn();

-    z += sigma * (real32_T)b;

-  }

-

-  if (posUpdate != 0.0F) {

-    y = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      b_y[i0] = 0.0F;

-      for (i1 = 0; i1 < 3; i1++) {

-        b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];

-      }

-

-      y += b_y[i0] * x_aposteriori_k[i0];

-    }

-

-    y = z - y;

-    b_k1[0] = k1;

-    b_k1[1] = k2;

-    b_k1[2] = k3;

-    for (i0 = 0; i0 < 3; i0++) {

-      f0 = 0.0F;

-      for (i1 = 0; i1 < 3; i1++) {

-        f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];

-      }

-

-      x_aposteriori[i0] = f0 + b_k1[i0] * y;

-    }

-  } else {

-    for (i0 = 0; i0 < 3; i0++) {

-      x_aposteriori[i0] = 0.0F;

-      for (i1 = 0; i1 < 3; i1++) {

-        x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];

-      }

-    }

-  }

-}

-

-/* End of code generation (kalman_dlqe3.c) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h
deleted file mode 100755
index 9bbffbbb3..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*

- * kalman_dlqe3.h

- *

- * Code generation for function 'kalman_dlqe3'

- *

- * C source code generated on: Tue Feb 19 15:26:32 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_H__

-#define __KALMAN_DLQE3_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);

-#endif

-/* End of code generation (kalman_dlqe3.h) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c
deleted file mode 100755
index 8f2275c13..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c
+++ /dev/null
@@ -1,32 +0,0 @@
-/*

- * kalman_dlqe3_data.c

- *

- * Code generation for function 'kalman_dlqe3_data'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "kalman_dlqe3_data.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-uint32_T method;

-uint32_T state[2];

-uint32_T b_method;

-uint32_T b_state;

-uint32_T c_state[2];

-boolean_T state_not_empty;

-

-/* Function Declarations */

-

-/* Function Definitions */

-/* End of code generation (kalman_dlqe3_data.c) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h
deleted file mode 100755
index 952eb7b89..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*

- * kalman_dlqe3_data.h

- *

- * Code generation for function 'kalman_dlqe3_data'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_DATA_H__

-#define __KALMAN_DLQE3_DATA_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-extern uint32_T method;

-extern uint32_T state[2];

-extern uint32_T b_method;

-extern uint32_T b_state;

-extern uint32_T c_state[2];

-extern boolean_T state_not_empty;

-

-/* Variable Definitions */

-

-/* Function Declarations */

-#endif

-/* End of code generation (kalman_dlqe3_data.h) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
deleted file mode 100755
index b87d604c4..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
+++ /dev/null
@@ -1,47 +0,0 @@
-/*

- * kalman_dlqe3_initialize.c

- *

- * Code generation for function 'kalman_dlqe3_initialize'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "kalman_dlqe3_initialize.h"

-#include "kalman_dlqe3_data.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void kalman_dlqe3_initialize(void)

-{

-  int32_T i;

-  static const uint32_T uv0[2] = { 362436069U, 0U };

-

-  rt_InitInfAndNaN(8U);

-  state_not_empty = FALSE;

-  b_state = 1144108930U;

-  b_method = 7U;

-  method = 0U;

-  for (i = 0; i < 2; i++) {

-    c_state[i] = 362436069U + 158852560U * (uint32_T)i;

-    state[i] = uv0[i];

-  }

-

-  if (state[1] == 0U) {

-    state[1] = 521288629U;

-  }

-}

-

-/* End of code generation (kalman_dlqe3_initialize.c) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
deleted file mode 100755
index 9dee90f9e..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*

- * kalman_dlqe3_initialize.h

- *

- * Code generation for function 'kalman_dlqe3_initialize'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_INITIALIZE_H__

-#define __KALMAN_DLQE3_INITIALIZE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void kalman_dlqe3_initialize(void);

-#endif

-/* End of code generation (kalman_dlqe3_initialize.h) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
deleted file mode 100755
index b00858205..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * kalman_dlqe3_terminate.c

- *

- * Code generation for function 'kalman_dlqe3_terminate'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "kalman_dlqe3_terminate.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void kalman_dlqe3_terminate(void)

-{

-  /* (no terminate code required) */

-}

-

-/* End of code generation (kalman_dlqe3_terminate.c) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
deleted file mode 100755
index 69cc85c76..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*

- * kalman_dlqe3_terminate.h

- *

- * Code generation for function 'kalman_dlqe3_terminate'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_TERMINATE_H__

-#define __KALMAN_DLQE3_TERMINATE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void kalman_dlqe3_terminate(void);

-#endif

-/* End of code generation (kalman_dlqe3_terminate.h) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h
deleted file mode 100755
index f36ea4557..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*

- * kalman_dlqe3_types.h

- *

- * Code generation for function 'kalman_dlqe3'

- *

- * C source code generated on: Tue Feb 19 15:26:30 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_TYPES_H__

-#define __KALMAN_DLQE3_TYPES_H__

-

-/* Type Definitions */

-

-#endif

-/* End of code generation (kalman_dlqe3_types.h) */

diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
deleted file mode 100755
index 5139848bc..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * positionKalmanFilter1D.c
- *
- * Code generation for function 'positionKalmanFilter1D'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
-  real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
-  P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
-  Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
-  real32_T P_aposteriori[9])
-{
-  int32_T i0;
-  real32_T f0;
-  int32_T k;
-  real32_T b_A[9];
-  int32_T i1;
-  real32_T P_apriori[9];
-  real32_T y;
-  real32_T K[3];
-  real32_T S;
-  int8_T I[9];
-
-  /* prediction */
-  for (i0 = 0; i0 < 3; i0++) {
-    f0 = 0.0F;
-    for (k = 0; k < 3; k++) {
-      f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
-    }
-
-    x_aposteriori[i0] = f0 + B[i0] * u;
-  }
-
-  for (i0 = 0; i0 < 3; i0++) {
-    for (k = 0; k < 3; k++) {
-      b_A[i0 + 3 * k] = 0.0F;
-      for (i1 = 0; i1 < 3; i1++) {
-        b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
-      }
-    }
-  }
-
-  for (i0 = 0; i0 < 3; i0++) {
-    for (k = 0; k < 3; k++) {
-      f0 = 0.0F;
-      for (i1 = 0; i1 < 3; i1++) {
-        f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
-      }
-
-      P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
-    }
-  }
-
-  if ((real32_T)fabs(u) < thresh) {
-    x_aposteriori[1] *= decay;
-  }
-
-  /* update */
-  if (gps_update == 1) {
-    y = 0.0F;
-    for (k = 0; k < 3; k++) {
-      y += C[k] * x_aposteriori[k];
-      K[k] = 0.0F;
-      for (i0 = 0; i0 < 3; i0++) {
-        K[k] += C[i0] * P_apriori[i0 + 3 * k];
-      }
-    }
-
-    y = z - y;
-    S = 0.0F;
-    for (k = 0; k < 3; k++) {
-      S += K[k] * C[k];
-    }
-
-    S += R;
-    for (i0 = 0; i0 < 3; i0++) {
-      f0 = 0.0F;
-      for (k = 0; k < 3; k++) {
-        f0 += P_apriori[i0 + 3 * k] * C[k];
-      }
-
-      K[i0] = f0 / S;
-    }
-
-    for (i0 = 0; i0 < 3; i0++) {
-      x_aposteriori[i0] += K[i0] * y;
-    }
-
-    for (i0 = 0; i0 < 9; i0++) {
-      I[i0] = 0;
-    }
-
-    for (k = 0; k < 3; k++) {
-      I[k + 3 * k] = 1;
-    }
-
-    for (i0 = 0; i0 < 3; i0++) {
-      for (k = 0; k < 3; k++) {
-        b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
-      }
-    }
-
-    for (i0 = 0; i0 < 3; i0++) {
-      for (k = 0; k < 3; k++) {
-        P_aposteriori[i0 + 3 * k] = 0.0F;
-        for (i1 = 0; i1 < 3; i1++) {
-          P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
-        }
-      }
-    }
-  } else {
-    for (i0 = 0; i0 < 9; i0++) {
-      P_aposteriori[i0] = P_apriori[i0];
-    }
-  }
-}
-
-/* End of code generation (positionKalmanFilter1D.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h
deleted file mode 100755
index 205c8eb4e..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D.h
- *
- * Code generation for function 'positionKalmanFilter1D'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_H__
-#define __POSITIONKALMANFILTER1D_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
-#endif
-/* End of code generation (positionKalmanFilter1D.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
deleted file mode 100755
index 4c535618a..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
+++ /dev/null
@@ -1,157 +0,0 @@
-/*
- * positionKalmanFilter1D_dT.c
- *
- * Code generation for function 'positionKalmanFilter1D_dT'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D_dT.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3],
-  const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update,
-  const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T
-  x_aposteriori[3], real32_T P_aposteriori[9])
-{
-  real32_T A[9];
-  int32_T i;
-  static const int8_T iv0[3] = { 0, 0, 1 };
-
-  real32_T K[3];
-  real32_T f0;
-  int32_T i0;
-  real32_T b_A[9];
-  int32_T i1;
-  real32_T P_apriori[9];
-  static const int8_T iv1[3] = { 1, 0, 0 };
-
-  real32_T fv0[3];
-  real32_T y;
-  static const int8_T iv2[3] = { 1, 0, 0 };
-
-  real32_T S;
-  int8_T I[9];
-
-  /* dynamics */
-  A[0] = 1.0F;
-  A[3] = dT;
-  A[6] = -0.5F * dT * dT;
-  A[1] = 0.0F;
-  A[4] = 1.0F;
-  A[7] = -dT;
-  for (i = 0; i < 3; i++) {
-    A[2 + 3 * i] = (real32_T)iv0[i];
-  }
-
-  /* prediction */
-  K[0] = 0.5F * dT * dT;
-  K[1] = dT;
-  K[2] = 0.0F;
-  for (i = 0; i < 3; i++) {
-    f0 = 0.0F;
-    for (i0 = 0; i0 < 3; i0++) {
-      f0 += A[i + 3 * i0] * x_aposteriori_k[i0];
-    }
-
-    x_aposteriori[i] = f0 + K[i] * u;
-  }
-
-  for (i = 0; i < 3; i++) {
-    for (i0 = 0; i0 < 3; i0++) {
-      b_A[i + 3 * i0] = 0.0F;
-      for (i1 = 0; i1 < 3; i1++) {
-        b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0];
-      }
-    }
-  }
-
-  for (i = 0; i < 3; i++) {
-    for (i0 = 0; i0 < 3; i0++) {
-      f0 = 0.0F;
-      for (i1 = 0; i1 < 3; i1++) {
-        f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1];
-      }
-
-      P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0];
-    }
-  }
-
-  if ((real32_T)fabs(u) < thresh) {
-    x_aposteriori[1] *= decay;
-  }
-
-  /* update */
-  if (gps_update == 1) {
-    f0 = 0.0F;
-    for (i = 0; i < 3; i++) {
-      f0 += (real32_T)iv1[i] * x_aposteriori[i];
-      fv0[i] = 0.0F;
-      for (i0 = 0; i0 < 3; i0++) {
-        fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i];
-      }
-    }
-
-    y = z - f0;
-    f0 = 0.0F;
-    for (i = 0; i < 3; i++) {
-      f0 += fv0[i] * (real32_T)iv2[i];
-    }
-
-    S = f0 + (real32_T)R;
-    for (i = 0; i < 3; i++) {
-      f0 = 0.0F;
-      for (i0 = 0; i0 < 3; i0++) {
-        f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0];
-      }
-
-      K[i] = f0 / S;
-    }
-
-    for (i = 0; i < 3; i++) {
-      x_aposteriori[i] += K[i] * y;
-    }
-
-    for (i = 0; i < 9; i++) {
-      I[i] = 0;
-    }
-
-    for (i = 0; i < 3; i++) {
-      I[i + 3 * i] = 1;
-    }
-
-    for (i = 0; i < 3; i++) {
-      for (i0 = 0; i0 < 3; i0++) {
-        A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i];
-      }
-    }
-
-    for (i = 0; i < 3; i++) {
-      for (i0 = 0; i0 < 3; i0++) {
-        P_aposteriori[i + 3 * i0] = 0.0F;
-        for (i1 = 0; i1 < 3; i1++) {
-          P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0];
-        }
-      }
-    }
-  } else {
-    for (i = 0; i < 9; i++) {
-      P_aposteriori[i] = P_apriori[i];
-    }
-  }
-}
-
-/* End of code generation (positionKalmanFilter1D_dT.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
deleted file mode 100755
index 94cbe2ce6..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT.h
- *
- * Code generation for function 'positionKalmanFilter1D_dT'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_DT_H__
-#define __POSITIONKALMANFILTER1D_DT_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_dT_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
-#endif
-/* End of code generation (positionKalmanFilter1D_dT.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
deleted file mode 100755
index aa89f8a9d..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_initialize.c
- *
- * Code generation for function 'positionKalmanFilter1D_dT_initialize'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D_dT.h"
-#include "positionKalmanFilter1D_dT_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_dT_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
deleted file mode 100755
index 8d358a9a3..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_initialize.h
- *
- * Code generation for function 'positionKalmanFilter1D_dT_initialize'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
-#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_dT_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_dT_initialize(void);
-#endif
-/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
deleted file mode 100755
index 20ed2edbb..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_terminate.c
- *
- * Code generation for function 'positionKalmanFilter1D_dT_terminate'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D_dT.h"
-#include "positionKalmanFilter1D_dT_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_dT_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
deleted file mode 100755
index 5eb5807a0..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_terminate.h
- *
- * Code generation for function 'positionKalmanFilter1D_dT_terminate'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
-#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_dT_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_dT_terminate(void);
-#endif
-/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
deleted file mode 100755
index 43e5f016c..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_types.h
- *
- * Code generation for function 'positionKalmanFilter1D_dT'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__
-#define __POSITIONKALMANFILTER1D_DT_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (positionKalmanFilter1D_dT_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
deleted file mode 100755
index 5bd87c390..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_initialize.c
- *
- * Code generation for function 'positionKalmanFilter1D_initialize'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D.h"
-#include "positionKalmanFilter1D_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (positionKalmanFilter1D_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
deleted file mode 100755
index 44bce472f..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_initialize.h
- *
- * Code generation for function 'positionKalmanFilter1D_initialize'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__
-#define __POSITIONKALMANFILTER1D_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_initialize(void);
-#endif
-/* End of code generation (positionKalmanFilter1D_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
deleted file mode 100755
index 41e11936f..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_terminate.c
- *
- * Code generation for function 'positionKalmanFilter1D_terminate'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D.h"
-#include "positionKalmanFilter1D_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (positionKalmanFilter1D_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
deleted file mode 100755
index e84ea01bc..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_terminate.h
- *
- * Code generation for function 'positionKalmanFilter1D_terminate'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__
-#define __POSITIONKALMANFILTER1D_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_terminate(void);
-#endif
-/* End of code generation (positionKalmanFilter1D_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
deleted file mode 100755
index 4b473f56f..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * positionKalmanFilter1D_types.h
- *
- * Code generation for function 'positionKalmanFilter1D'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_TYPES_H__
-#define __POSITIONKALMANFILTER1D_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (positionKalmanFilter1D_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/randn.c b/src/modules/position_estimator_mc/codegen/randn.c
deleted file mode 100755
index 51aef7b76..000000000
--- a/src/modules/position_estimator_mc/codegen/randn.c
+++ /dev/null
@@ -1,524 +0,0 @@
-/*

- * randn.c

- *

- * Code generation for function 'randn'

- *

- * C source code generated on: Tue Feb 19 15:26:32 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "randn.h"

-#include "kalman_dlqe3_data.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-static uint32_T d_state[625];

-

-/* Function Declarations */

-static real_T b_genrandu(uint32_T mt[625]);

-static real_T eml_rand_mt19937ar(uint32_T e_state[625]);

-static real_T eml_rand_shr3cong(uint32_T e_state[2]);

-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);

-static void genrandu(uint32_T s, uint32_T *e_state, real_T *r);

-static void twister_state_vector(uint32_T mt[625], real_T seed);

-

-/* Function Definitions */

-static real_T b_genrandu(uint32_T mt[625])

-{

-  real_T r;

-  int32_T exitg1;

-  uint32_T u[2];

-  boolean_T isvalid;

-  int32_T k;

-  boolean_T exitg2;

-

-  /* <LEGAL>   This is a uniform (0,1) pseudorandom number generator based on: */

-  /* <LEGAL> */

-  /* <LEGAL>    A C-program for MT19937, with initialization improved 2002/1/26. */

-  /* <LEGAL>    Coded by Takuji Nishimura and Makoto Matsumoto. */

-  /* <LEGAL> */

-  /* <LEGAL>    Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */

-  /* <LEGAL>    All rights reserved. */

-  /* <LEGAL> */

-  /* <LEGAL>    Redistribution and use in source and binary forms, with or without */

-  /* <LEGAL>    modification, are permitted provided that the following conditions */

-  /* <LEGAL>    are met: */

-  /* <LEGAL> */

-  /* <LEGAL>      1. Redistributions of source code must retain the above copyright */

-  /* <LEGAL>         notice, this list of conditions and the following disclaimer. */

-  /* <LEGAL> */

-  /* <LEGAL>      2. Redistributions in binary form must reproduce the above copyright */

-  /* <LEGAL>         notice, this list of conditions and the following disclaimer in the */

-  /* <LEGAL>         documentation and/or other materials provided with the distribution. */

-  /* <LEGAL> */

-  /* <LEGAL>      3. The names of its contributors may not be used to endorse or promote */

-  /* <LEGAL>         products derived from this software without specific prior written */

-  /* <LEGAL>         permission. */

-  /* <LEGAL> */

-  /* <LEGAL>    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */

-  /* <LEGAL>    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */

-  /* <LEGAL>    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */

-  /* <LEGAL>    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR */

-  /* <LEGAL>    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */

-  /* <LEGAL>    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */

-  /* <LEGAL>    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */

-  /* <LEGAL>    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */

-  /* <LEGAL>    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */

-  /* <LEGAL>    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */

-  /* <LEGAL>    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */

-  do {

-    exitg1 = 0;

-    genrand_uint32_vector(mt, u);

-    r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)

-                                  (u[1] >> 6U));

-    if (r == 0.0) {

-      if ((mt[624] >= 1U) && (mt[624] < 625U)) {

-        isvalid = TRUE;

-      } else {

-        isvalid = FALSE;

-      }

-

-      if (isvalid) {

-        isvalid = FALSE;

-        k = 1;

-        exitg2 = FALSE;

-        while ((exitg2 == FALSE) && (k < 625)) {

-          if (mt[k - 1] == 0U) {

-            k++;

-          } else {

-            isvalid = TRUE;

-            exitg2 = TRUE;

-          }

-        }

-      }

-

-      if (!isvalid) {

-        twister_state_vector(mt, 5489.0);

-      }

-    } else {

-      exitg1 = 1;

-    }

-  } while (exitg1 == 0);

-

-  return r;

-}

-

-static real_T eml_rand_mt19937ar(uint32_T e_state[625])

-{

-  real_T r;

-  int32_T exitg1;

-  uint32_T u32[2];

-  int32_T i;

-  static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068,

-    0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,

-    0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,

-    0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,

-    0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,

-    0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,

-    0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,

-    0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,

-    0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,

-    0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,

-    0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,

-    0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,

-    0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,

-    0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,

-    1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,

-    1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,

-    1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,

-    1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,

-    1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,

-    1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,

-    1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,

-    1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,

-    1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,

-    1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,

-    1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,

-    1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,

-    1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,

-    1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,

-    1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,

-    1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,

-    1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,

-    1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,

-    1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,

-    1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,

-    1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,

-    1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,

-    1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,

-    1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,

-    1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,

-    1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,

-    1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,

-    1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,

-    1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,

-    1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,

-    1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,

-    1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,

-    1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,

-    1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,

-    2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,

-    2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,

-    2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,

-    2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,

-    2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,

-    2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,

-    2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,

-    2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,

-    2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,

-    2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,

-    2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,

-    2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,

-    2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,

-    2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,

-    2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,

-    3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,

-    3.65415288536101, 3.91075795952492 };

-

-  real_T u;

-  static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108,

-    0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,

-    0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,

-    0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,

-    0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,

-    0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,

-    0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,

-    0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,

-    0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,

-    0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,

-    0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,

-    0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,

-    0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,

-    0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,

-    0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,

-    0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,

-    0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,

-    0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,

-    0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,

-    0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,

-    0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,

-    0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,

-    0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,

-    0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,

-    0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,

-    0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,

-    0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,

-    0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,

-    0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,

-    0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,

-    0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,

-    0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,

-    0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,

-    0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,

-    0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,

-    0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,

-    0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,

-    0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,

-    0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,

-    0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,

-    0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,

-    0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,

-    0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,

-    0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,

-    0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,

-    0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,

-    0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,

-    0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,

-    0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,

-    0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,

-    0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,

-    0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,

-    0.093365311912691, 0.0911136480663738, 0.0888735920682759,

-    0.0866451944505581, 0.0844285095703535, 0.082223595813203,

-    0.0800305158146631, 0.0778493367020961, 0.0756801303589272,

-    0.0735229737139814, 0.0713779490588905, 0.0692451443970068,

-    0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,

-    0.0587679529209339, 0.0567106901062031, 0.0546664613248891,

-    0.0526354182767924, 0.0506177238609479, 0.0486135532158687,

-    0.0466230949019305, 0.0446465522512946, 0.0426841449164746,

-    0.0407361106559411, 0.0388027074045262, 0.0368842156885674,

-    0.0349809414617162, 0.0330932194585786, 0.0312214171919203,

-    0.0293659397581334, 0.0275272356696031, 0.0257058040085489,

-    0.0239022033057959, 0.0221170627073089, 0.0203510962300445,

-    0.0186051212757247, 0.0168800831525432, 0.0151770883079353,

-    0.0134974506017399, 0.0118427578579079, 0.0102149714397015,

-    0.00861658276939875, 0.00705087547137324, 0.00552240329925101,

-    0.00403797259336304, 0.00260907274610216, 0.0012602859304986,

-    0.000477467764609386 };

-

-  real_T x;

-  do {

-    exitg1 = 0;

-    genrand_uint32_vector(e_state, u32);

-    i = (int32_T)((u32[1] >> 24U) + 1U);

-    r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &

-           16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i];

-    if (fabs(r) <= dv1[i - 1]) {

-      exitg1 = 1;

-    } else if (i < 256) {

-      u = b_genrandu(e_state);

-      if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) {

-        exitg1 = 1;

-      }

-    } else {

-      do {

-        u = b_genrandu(e_state);

-        x = log(u) * 0.273661237329758;

-        u = b_genrandu(e_state);

-      } while (!(-2.0 * log(u) > x * x));

-

-      if (r < 0.0) {

-        r = x - 3.65415288536101;

-      } else {

-        r = 3.65415288536101 - x;

-      }

-

-      exitg1 = 1;

-    }

-  } while (exitg1 == 0);

-

-  return r;

-}

-

-static real_T eml_rand_shr3cong(uint32_T e_state[2])

-{

-  real_T r;

-  uint32_T icng;

-  uint32_T jsr;

-  uint32_T ui;

-  int32_T j;

-  static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,

-    0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,

-    0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,

-    1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,

-    1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,

-    1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,

-    1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,

-    1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,

-    2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,

-    2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };

-

-  real_T x;

-  real_T y;

-  real_T s;

-  icng = 69069U * e_state[0] + 1234567U;

-  jsr = e_state[1] ^ e_state[1] << 13U;

-  jsr ^= jsr >> 17U;

-  jsr ^= jsr << 5U;

-  ui = icng + jsr;

-  j = (int32_T)(ui & 63U);

-  r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1];

-  if (fabs(r) <= dv0[j]) {

-  } else {

-    x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]);

-    icng = 69069U * icng + 1234567U;

-    jsr ^= jsr << 13U;

-    jsr ^= jsr >> 17U;

-    jsr ^= jsr << 5U;

-    y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10;

-    s = x + (0.5 + y);

-    if (s > 1.301198) {

-      if (r < 0.0) {

-        r = 0.4878992 * x - 0.4878992;

-      } else {

-        r = 0.4878992 - 0.4878992 * x;

-      }

-    } else if (s <= 0.9689279) {

-    } else {

-      s = 0.4878992 * x;

-      x = 0.4878992 - 0.4878992 * x;

-      if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) {

-        if (r < 0.0) {

-          r = -(0.4878992 - s);

-        } else {

-          r = 0.4878992 - s;

-        }

-      } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 /

-                 dv0[j + 1] <= exp(-0.5 * r * r)) {

-      } else {

-        do {

-          icng = 69069U * icng + 1234567U;

-          jsr ^= jsr << 13U;

-          jsr ^= jsr >> 17U;

-          jsr ^= jsr << 5U;

-          x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) /

-            2.776994;

-          icng = 69069U * icng + 1234567U;

-          jsr ^= jsr << 13U;

-          jsr ^= jsr >> 17U;

-          jsr ^= jsr << 5U;

-        } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) *

-                              2.328306436538696E-10) > x * x));

-

-        if (r < 0.0) {

-          r = x - 2.776994;

-        } else {

-          r = 2.776994 - x;

-        }

-      }

-    }

-  }

-

-  e_state[0] = icng;

-  e_state[1] = jsr;

-  return r;

-}

-

-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])

-{

-  int32_T i;

-  uint32_T mti;

-  int32_T kk;

-  uint32_T y;

-  uint32_T b_y;

-  uint32_T c_y;

-  uint32_T d_y;

-  for (i = 0; i < 2; i++) {

-    u[i] = 0U;

-  }

-

-  for (i = 0; i < 2; i++) {

-    mti = mt[624] + 1U;

-    if (mti >= 625U) {

-      for (kk = 0; kk < 227; kk++) {

-        y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);

-        if ((int32_T)(y & 1U) == 0) {

-          b_y = y >> 1U;

-        } else {

-          b_y = y >> 1U ^ 2567483615U;

-        }

-

-        mt[kk] = mt[397 + kk] ^ b_y;

-      }

-

-      for (kk = 0; kk < 396; kk++) {

-        y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);

-        if ((int32_T)(y & 1U) == 0) {

-          c_y = y >> 1U;

-        } else {

-          c_y = y >> 1U ^ 2567483615U;

-        }

-

-        mt[227 + kk] = mt[kk] ^ c_y;

-      }

-

-      y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);

-      if ((int32_T)(y & 1U) == 0) {

-        d_y = y >> 1U;

-      } else {

-        d_y = y >> 1U ^ 2567483615U;

-      }

-

-      mt[623] = mt[396] ^ d_y;

-      mti = 1U;

-    }

-

-    y = mt[(int32_T)mti - 1];

-    mt[624] = mti;

-    y ^= y >> 11U;

-    y ^= y << 7U & 2636928640U;

-    y ^= y << 15U & 4022730752U;

-    y ^= y >> 18U;

-    u[i] = y;

-  }

-}

-

-static void genrandu(uint32_T s, uint32_T *e_state, real_T *r)

-{

-  int32_T hi;

-  uint32_T test1;

-  uint32_T test2;

-  hi = (int32_T)(s / 127773U);

-  test1 = 16807U * (s - (uint32_T)hi * 127773U);

-  test2 = 2836U * (uint32_T)hi;

-  if (test1 < test2) {

-    *e_state = (test1 - test2) + 2147483647U;

-  } else {

-    *e_state = test1 - test2;

-  }

-

-  *r = (real_T)*e_state * 4.6566128752457969E-10;

-}

-

-static void twister_state_vector(uint32_T mt[625], real_T seed)

-{

-  uint32_T r;

-  int32_T mti;

-  if (seed < 4.294967296E+9) {

-    if (seed >= 0.0) {

-      r = (uint32_T)seed;

-    } else {

-      r = 0U;

-    }

-  } else if (seed >= 4.294967296E+9) {

-    r = MAX_uint32_T;

-  } else {

-    r = 0U;

-  }

-

-  mt[0] = r;

-  for (mti = 0; mti < 623; mti++) {

-    r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);

-    mt[1 + mti] = r;

-  }

-

-  mt[624] = 624U;

-}

-

-real_T randn(void)

-{

-  real_T r;

-  uint32_T e_state;

-  real_T t;

-  real_T b_r;

-  uint32_T f_state;

-  if (method == 0U) {

-    if (b_method == 4U) {

-      do {

-        genrandu(b_state, &e_state, &r);

-        genrandu(e_state, &b_state, &t);

-        b_r = 2.0 * r - 1.0;

-        t = 2.0 * t - 1.0;

-        t = t * t + b_r * b_r;

-      } while (!(t <= 1.0));

-

-      r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);

-    } else if (b_method == 5U) {

-      r = eml_rand_shr3cong(c_state);

-    } else {

-      if (!state_not_empty) {

-        memset(&d_state[0], 0, 625U * sizeof(uint32_T));

-        twister_state_vector(d_state, 5489.0);

-        state_not_empty = TRUE;

-      }

-

-      r = eml_rand_mt19937ar(d_state);

-    }

-  } else if (method == 4U) {

-    e_state = state[0];

-    do {

-      genrandu(e_state, &f_state, &r);

-      genrandu(f_state, &e_state, &t);

-      b_r = 2.0 * r - 1.0;

-      t = 2.0 * t - 1.0;

-      t = t * t + b_r * b_r;

-    } while (!(t <= 1.0));

-

-    state[0] = e_state;

-    r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);

-  } else {

-    r = eml_rand_shr3cong(state);

-  }

-

-  return r;

-}

-

-/* End of code generation (randn.c) */

diff --git a/src/modules/position_estimator_mc/codegen/randn.h b/src/modules/position_estimator_mc/codegen/randn.h
deleted file mode 100755
index 8a2aa9277..000000000
--- a/src/modules/position_estimator_mc/codegen/randn.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*

- * randn.h

- *

- * Code generation for function 'randn'

- *

- * C source code generated on: Tue Feb 19 15:26:32 2013

- *

- */

-

-#ifndef __RANDN_H__

-#define __RANDN_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern real_T randn(void);

-#endif

-/* End of code generation (randn.h) */

diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.c b/src/modules/position_estimator_mc/codegen/rtGetInf.c
deleted file mode 100755
index c6fa7884e..000000000
--- a/src/modules/position_estimator_mc/codegen/rtGetInf.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
- * rtGetInf.c
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-/*
- * Abstract:
- *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf
- */
-#include "rtGetInf.h"
-#define NumBitsPerChar	8U
-
-/* Function: rtGetInf ==================================================
- * Abstract:
- * Initialize rtInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetInf(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T inf = 0.0;
-  if (bitsPerReal == 32U) {
-    inf = rtGetInfF();
-  } else {
-    uint16_T one = 1U;
-    enum {
-      LittleEndian,
-      BigEndian
-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-    switch (machByteOrder) {
-     case LittleEndian:
-      {
-        union {
-          LittleEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        inf = tmpVal.fltVal;
-        break;
-      }
-
-     case BigEndian:
-      {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        inf = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return inf;
-}
-
-/* Function: rtGetInfF ==================================================
- * Abstract:
- * Initialize rtInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetInfF(void)
-{
-  IEEESingle infF;
-  infF.wordL.wordLuint = 0x7F800000U;
-  return infF.wordL.wordLreal;
-}
-
-/* Function: rtGetMinusInf ==================================================
- * Abstract:
- * Initialize rtMinusInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetMinusInf(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T minf = 0.0;
-  if (bitsPerReal == 32U) {
-    minf = rtGetMinusInfF();
-  } else {
-    uint16_T one = 1U;
-    enum {
-      LittleEndian,
-      BigEndian
-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-    switch (machByteOrder) {
-     case LittleEndian:
-      {
-        union {
-          LittleEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0xFFF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        minf = tmpVal.fltVal;
-        break;
-      }
-
-     case BigEndian:
-      {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0xFFF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        minf = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return minf;
-}
-
-/* Function: rtGetMinusInfF ==================================================
- * Abstract:
- * Initialize rtMinusInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetMinusInfF(void)
-{
-  IEEESingle minfF;
-  minfF.wordL.wordLuint = 0xFF800000U;
-  return minfF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetInf.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.h b/src/modules/position_estimator_mc/codegen/rtGetInf.h
deleted file mode 100755
index e7b2a2d1c..000000000
--- a/src/modules/position_estimator_mc/codegen/rtGetInf.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*
- * rtGetInf.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __RTGETINF_H__
-#define __RTGETINF_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetInf(void);
-extern real32_T rtGetInfF(void);
-extern real_T rtGetMinusInf(void);
-extern real32_T rtGetMinusInfF(void);
-
-#endif
-/* End of code generation (rtGetInf.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.c b/src/modules/position_estimator_mc/codegen/rtGetNaN.c
deleted file mode 100755
index 552770149..000000000
--- a/src/modules/position_estimator_mc/codegen/rtGetNaN.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * rtGetNaN.c
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-/*
- * Abstract:
- *       MATLAB for code generation function to initialize non-finite, NaN
- */
-#include "rtGetNaN.h"
-#define NumBitsPerChar	8U
-
-/* Function: rtGetNaN ==================================================
- * Abstract:
- * Initialize rtNaN needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetNaN(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T nan = 0.0;
-  if (bitsPerReal == 32U) {
-    nan = rtGetNaNF();
-  } else {
-  uint16_T one = 1U;
-  enum {
-    LittleEndian,
-    BigEndian
-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-  switch (machByteOrder) {
-   case LittleEndian:
-    {
-      union {
-        LittleEndianIEEEDouble bitVal;
-        real_T fltVal;
-      } tmpVal;
-
-      tmpVal.bitVal.words.wordH = 0xFFF80000U;
-      tmpVal.bitVal.words.wordL = 0x00000000U;
-      nan = tmpVal.fltVal;
-      break;
-    }
-
-   case BigEndian:
-    {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
-        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
-        nan = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return nan;
-}
-
-/* Function: rtGetNaNF ==================================================
- * Abstract:
- * Initialize rtNaNF needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetNaNF(void)
-{
-  IEEESingle nanF = { { 0 } };
-  uint16_T one = 1U;
-  enum {
-    LittleEndian,
-    BigEndian
-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-  switch (machByteOrder) {
-   case LittleEndian:
-    {
-      nanF.wordL.wordLuint = 0xFFC00000U;
-      break;
-    }
-
-   case BigEndian:
-    {
-      nanF.wordL.wordLuint = 0x7FFFFFFFU;
-      break;
-    }
-  }
-
-  return nanF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetNaN.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.h b/src/modules/position_estimator_mc/codegen/rtGetNaN.h
deleted file mode 100755
index 5acdd9790..000000000
--- a/src/modules/position_estimator_mc/codegen/rtGetNaN.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * rtGetNaN.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __RTGETNAN_H__
-#define __RTGETNAN_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetNaN(void);
-extern real32_T rtGetNaNF(void);
-
-#endif
-/* End of code generation (rtGetNaN.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c
deleted file mode 100755
index de121c4a0..000000000
--- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * rt_nonfinite.c
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-/*
- * Abstract:
- *      MATLAB for code generation function to initialize non-finites,
- *      (Inf, NaN and -Inf).
- */
-#include "rt_nonfinite.h"
-#include "rtGetNaN.h"
-#include "rtGetInf.h"
-
-real_T rtInf;
-real_T rtMinusInf;
-real_T rtNaN;
-real32_T rtInfF;
-real32_T rtMinusInfF;
-real32_T rtNaNF;
-
-/* Function: rt_InitInfAndNaN ==================================================
- * Abstract:
- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
- * generated code. NaN is initialized as non-signaling. Assumes IEEE.
- */
-void rt_InitInfAndNaN(size_t realSize)
-{
-  (void) (realSize);
-  rtNaN = rtGetNaN();
-  rtNaNF = rtGetNaNF();
-  rtInf = rtGetInf();
-  rtInfF = rtGetInfF();
-  rtMinusInf = rtGetMinusInf();
-  rtMinusInfF = rtGetMinusInfF();
-}
-
-/* Function: rtIsInf ==================================================
- * Abstract:
- * Test if value is infinite
- */
-boolean_T rtIsInf(real_T value)
-{
-  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
-}
-
-/* Function: rtIsInfF =================================================
- * Abstract:
- * Test if single-precision value is infinite
- */
-boolean_T rtIsInfF(real32_T value)
-{
-  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
-}
-
-/* Function: rtIsNaN ==================================================
- * Abstract:
- * Test if value is not a number
- */
-boolean_T rtIsNaN(real_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-  return _isnan(value)? TRUE:FALSE;
-#else
-  return (value!=value)? 1U:0U;
-#endif
-}
-
-/* Function: rtIsNaNF =================================================
- * Abstract:
- * Test if single-precision value is not a number
- */
-boolean_T rtIsNaNF(real32_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-  return _isnan((real_T)value)? true:false;
-#else
-  return (value!=value)? 1U:0U;
-#endif
-}
-
-
-/* End of code generation (rt_nonfinite.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h
deleted file mode 100755
index 3bbcfd087..000000000
--- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * rt_nonfinite.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __RT_NONFINITE_H__
-#define __RT_NONFINITE_H__
-
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-#include <float.h>
-#endif
-#include <stddef.h>
-#include "rtwtypes.h"
-
-extern real_T rtInf;
-extern real_T rtMinusInf;
-extern real_T rtNaN;
-extern real32_T rtInfF;
-extern real32_T rtMinusInfF;
-extern real32_T rtNaNF;
-extern void rt_InitInfAndNaN(size_t realSize);
-extern boolean_T rtIsInf(real_T value);
-extern boolean_T rtIsInfF(real32_T value);
-extern boolean_T rtIsNaN(real_T value);
-extern boolean_T rtIsNaNF(real32_T value);
-
-typedef struct {
-  struct {
-    uint32_T wordH;
-    uint32_T wordL;
-  } words;
-} BigEndianIEEEDouble;
-
-typedef struct {
-  struct {
-    uint32_T wordL;
-    uint32_T wordH;
-  } words;
-} LittleEndianIEEEDouble;
-
-typedef struct {
-  union {
-    real32_T wordLreal;
-    uint32_T wordLuint;
-  } wordL;
-} IEEESingle;
-
-#endif
-/* End of code generation (rt_nonfinite.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtwtypes.h b/src/modules/position_estimator_mc/codegen/rtwtypes.h
deleted file mode 100755
index 8916e8572..000000000
--- a/src/modules/position_estimator_mc/codegen/rtwtypes.h
+++ /dev/null
@@ -1,159 +0,0 @@
-/*
- * rtwtypes.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __RTWTYPES_H__
-#define __RTWTYPES_H__
-#ifndef TRUE
-# define TRUE (1U)
-#endif
-#ifndef FALSE
-# define FALSE (0U)
-#endif
-#ifndef __TMWTYPES__
-#define __TMWTYPES__
-
-#include <limits.h>
-
-/*=======================================================================* 
- * Target hardware information
- *   Device type: Generic->MATLAB Host Computer
- *   Number of bits:     char:   8    short:   16    int:  32
- *                       long:  32      native word size:  32
- *   Byte ordering: LittleEndian
- *   Signed integer division rounds to: Undefined
- *   Shift right on a signed integer as arithmetic shift: off
- *=======================================================================*/
-
-/*=======================================================================* 
- * Fixed width word size data types:                                     * 
- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
- *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 
- *=======================================================================*/
-
-typedef signed char int8_T;
-typedef unsigned char uint8_T;
-typedef short int16_T;
-typedef unsigned short uint16_T;
-typedef int int32_T;
-typedef unsigned int uint32_T;
-typedef float real32_T;
-typedef double real64_T;
-
-/*===========================================================================* 
- * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 
- *                           ulong_T, char_T and byte_T.                     * 
- *===========================================================================*/
-
-typedef double real_T;
-typedef double time_T;
-typedef unsigned char boolean_T;
-typedef int int_T;
-typedef unsigned int uint_T;
-typedef unsigned long ulong_T;
-typedef char char_T;
-typedef char_T byte_T;
-
-/*===========================================================================* 
- * Complex number type definitions                                           * 
- *===========================================================================*/
-#define CREAL_T	
-   typedef struct {  
-     real32_T re;  
-     real32_T im;  
-   } creal32_T;  
-
-   typedef struct {  
-     real64_T re;  
-     real64_T im;  
-   } creal64_T;  
-
-   typedef struct {  
-     real_T re;  
-     real_T im;  
-   } creal_T;  
-
-   typedef struct {  
-     int8_T re;  
-     int8_T im;  
-   } cint8_T;  
-
-   typedef struct {  
-     uint8_T re;  
-     uint8_T im;  
-   } cuint8_T;  
-
-   typedef struct {  
-     int16_T re;  
-     int16_T im;  
-   } cint16_T;  
-
-   typedef struct {  
-     uint16_T re;  
-     uint16_T im;  
-   } cuint16_T;  
-
-   typedef struct {  
-     int32_T re;  
-     int32_T im;  
-   } cint32_T;  
-
-   typedef struct {  
-     uint32_T re;  
-     uint32_T im;  
-   } cuint32_T;  
-
-
-/*=======================================================================* 
- * Min and Max:                                                          * 
- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
- *=======================================================================*/
-
-#define MAX_int8_T  	((int8_T)(127))
-#define MIN_int8_T  	((int8_T)(-128))
-#define MAX_uint8_T 	((uint8_T)(255))
-#define MIN_uint8_T 	((uint8_T)(0))
-#define MAX_int16_T 	((int16_T)(32767))
-#define MIN_int16_T 	((int16_T)(-32768))
-#define MAX_uint16_T	((uint16_T)(65535))
-#define MIN_uint16_T	((uint16_T)(0))
-#define MAX_int32_T 	((int32_T)(2147483647))
-#define MIN_int32_T 	((int32_T)(-2147483647-1))
-#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))
-#define MIN_uint32_T	((uint32_T)(0))
-
-/* Logical type definitions */
-#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
-#  ifndef false
-#   define false (0U)
-#  endif
-#  ifndef true
-#   define true (1U)
-#  endif
-#endif
-
-/*
- * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
- * for signed integer values.
- */
-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
-#error "This code must be compiled using a 2's complement representation for signed integer values"
-#endif
-
-/*
- * Maximum length of a MATLAB identifier (function/variable)
- * including the null-termination character. Referenced by
- * rt_logging.c and rt_matrx.c.
- */
-#define TMW_NAME_LENGTH_MAX	64
-
-#endif
-#endif
-/* End of code generation (rtwtypes.h) */
diff --git a/src/modules/position_estimator_mc/kalman_dlqe1.m b/src/modules/position_estimator_mc/kalman_dlqe1.m
deleted file mode 100755
index ff939d029..000000000
--- a/src/modules/position_estimator_mc/kalman_dlqe1.m
+++ /dev/null
@@ -1,3 +0,0 @@
-function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z)
-    x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);      
-end
\ No newline at end of file
diff --git a/src/modules/position_estimator_mc/kalman_dlqe2.m b/src/modules/position_estimator_mc/kalman_dlqe2.m
deleted file mode 100755
index 2a50164ef..000000000
--- a/src/modules/position_estimator_mc/kalman_dlqe2.m
+++ /dev/null
@@ -1,9 +0,0 @@
-function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z)
-    st = 1/2*dt^2;
-    A = [1,dt,st;
-        0,1,dt;
-        0,0,1];
-    C=[1,0,0];
-    K = [k1;k2;k3];
-    x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);      
-end
\ No newline at end of file
diff --git a/src/modules/position_estimator_mc/kalman_dlqe3.m b/src/modules/position_estimator_mc/kalman_dlqe3.m
deleted file mode 100755
index 4c6421b7f..000000000
--- a/src/modules/position_estimator_mc/kalman_dlqe3.m
+++ /dev/null
@@ -1,17 +0,0 @@
-function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)

-    st = 1/2*dt^2;

-    A = [1,dt,st;

-        0,1,dt;

-        0,0,1];

-    C=[1,0,0];

-    K = [k1;k2;k3];

-    if addNoise==1

-        noise = sigma*randn(1,1);

-        z = z + noise;

-    end

-    if(posUpdate)

-        x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);

-    else

-        x_aposteriori=A*x_aposteriori_k;

-    end

-end
\ No newline at end of file
diff --git a/src/modules/position_estimator_mc/module.mk b/src/modules/position_estimator_mc/module.mk
deleted file mode 100644
index 40b135ea4..000000000
--- a/src/modules/position_estimator_mc/module.mk
+++ /dev/null
@@ -1,60 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 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.
-#
-############################################################################
-
-#
-# Makefile to build the position estimator
-#
-
-MODULE_COMMAND	 = position_estimator_mc
-
-SRCS		 = position_estimator_mc_main.c \
-			   position_estimator_mc_params.c \
-			   codegen/positionKalmanFilter1D_initialize.c \
-			   codegen/positionKalmanFilter1D_terminate.c \
-			   codegen/positionKalmanFilter1D.c \
-			   codegen/rt_nonfinite.c \
-			   codegen/rtGetInf.c \
-			   codegen/rtGetNaN.c \
-			   codegen/positionKalmanFilter1D_dT_initialize.c \
-			   codegen/positionKalmanFilter1D_dT_terminate.c \
-			   codegen/kalman_dlqe1.c \
-			   codegen/kalman_dlqe1_initialize.c \
-			   codegen/kalman_dlqe1_terminate.c \
-			   codegen/kalman_dlqe2.c \
-			   codegen/kalman_dlqe2_initialize.c \
-			   codegen/kalman_dlqe2_terminate.c \
-			   codegen/kalman_dlqe3.c \
-			   codegen/kalman_dlqe3_initialize.c \
-			   codegen/kalman_dlqe3_terminate.c \
-			   codegen/kalman_dlqe3_data.c \
-			   codegen/randn.c
diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D.m b/src/modules/position_estimator_mc/positionKalmanFilter1D.m
deleted file mode 100755
index 144ff8c7c..000000000
--- a/src/modules/position_estimator_mc/positionKalmanFilter1D.m
+++ /dev/null
@@ -1,19 +0,0 @@
-function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
-%prediction
-    x_apriori=A*x_aposteriori_k+B*u;
-    P_apriori=A*P_aposteriori_k*A'+Q;
-    if abs(u)<thresh
-        x_apriori(2)=decay*x_apriori(2);
-    end
-    %update
-    if gps_update==1
-        y=z-C*x_apriori;
-        S=C*P_apriori*C'+R;
-        K=(P_apriori*C')/S;
-        x_aposteriori=x_apriori+K*y;
-        P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
-    else
-        x_aposteriori=x_apriori;
-        P_aposteriori=P_apriori;
-    end
-end
diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m
deleted file mode 100755
index f94cce1fb..000000000
--- a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m
+++ /dev/null
@@ -1,26 +0,0 @@
-function  [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
-    %dynamics
-    A = [1 dT -0.5*dT*dT;
-         0 1 -dT;
-         0 0 1];
-    B = [0.5*dT*dT; dT; 0];
-    C = [1 0 0];
-    %prediction
-    x_apriori=A*x_aposteriori_k+B*u;
-    P_apriori=A*P_aposteriori_k*A'+Q;
-    if abs(u)<thresh
-        x_apriori(2)=decay*x_apriori(2);
-    end
-    %update
-    if gps_update==1
-        y=z-C*x_apriori;
-        S=C*P_apriori*C'+R;
-        K=(P_apriori*C')/S;
-        x_aposteriori=x_apriori+K*y;
-        P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
-    else
-        x_aposteriori=x_apriori;
-        P_aposteriori=P_apriori;
-    end
-end
-
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
deleted file mode 100755
index 363961819..000000000
--- a/src/modules/position_estimator_mc/position_estimator_mc_main.c
+++ /dev/null
@@ -1,515 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: 	Damian Aregger	<daregger@student.ethz.ch>
- *   			Tobias Naegeli <naegelit@student.ethz.ch>
-* 				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
- * 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 position_estimator_main.c
- * Model-identification based position estimator for multirotors
- */
-
-#include <unistd.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <float.h>
-#include <string.h>
-#include <nuttx/config.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <limits.h>
-#include <math.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls_effective.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <mavlink/mavlink_log.h>
-#include <poll.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/err.h>
-#include <systemlib/systemlib.h>
-
-#include <drivers/drv_hrt.h>
-
-#include "position_estimator_mc_params.h"
-//#include <uORB/topics/debug_key_value.h>
-#include "codegen/kalman_dlqe2.h"
-#include "codegen/kalman_dlqe2_initialize.h"
-#include "codegen/kalman_dlqe3.h"
-#include "codegen/kalman_dlqe3_initialize.h"
-
-static bool thread_should_exit = false;	/**< Deamon exit flag */
-static bool thread_running = false;	/**< Deamon status flag */
-static int position_estimator_mc_task;	/**< Handle of deamon task / thread */
-
-__EXPORT int position_estimator_mc_main(int argc, char *argv[]);
-
-int position_estimator_mc_thread_main(int argc, char *argv[]);
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		warnx("%s\n", reason);
-	warnx("usage: position_estimator_mc {start|stop|status}");
-	exit(1);
-}
-
-/**
- * The position_estimator_mc_thread only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int position_estimator_mc_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("position_estimator_mc already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		position_estimator_mc_task = task_spawn_cmd("position_estimator_mc",
-					 SCHED_RR,
-					 SCHED_PRIORITY_MAX - 5,
-					 4096,
-					 position_estimator_mc_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("position_estimator_mc is running");
-		} else {
-			warnx("position_estimator_mc not started");
-		}
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-/****************************************************************************
- * main
- ****************************************************************************/
-int position_estimator_mc_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	warnx("[position_estimator_mc] started");
-	int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[position_estimator_mc] started");
-
-	/* initialize values */
-	float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */
-	// float rotMatrix[4] = {1.0f,  0.0f, 0.0f,  1.0f};
-	float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
-	float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
-	float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
-	float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f};
-	float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f};
-	float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f};
-
-	// XXX this is terribly wrong and should actual dT instead
-	const float dT_const_50 = 1.0f/50.0f;
-
-	float addNoise = 0.0f;
-	float sigma = 0.0f;
-	//computed from dlqe in matlab
-	const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f};
-	// XXX implement baro filter
-	const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f};
-	float K[3] = {0.0f, 0.0f, 0.0f};
-	int baro_loop_cnt = 0;
-	int baro_loop_end = 70; /* measurement for 1 second */
-	float p0_Pa = 0.0f; /* to determin while start up */
-	float rho0 = 1.293f; /* standard pressure */
-	const float const_earth_gravity = 9.81f;
-
-	float posX = 0.0f;
-	float posY = 0.0f;
-	float posZ = 0.0f;
-
-	double lat_current;
-	double lon_current;
-	float alt_current;
-
-	float gps_origin_altitude = 0.0f;
-
-	/* Initialize filter */
-	kalman_dlqe2_initialize();
-	kalman_dlqe3_initialize();
-
-	/* declare and safely initialize all structs */
-	struct sensor_combined_s sensor;
-	memset(&sensor, 0, sizeof(sensor));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_status_s vehicle_status;
-	memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */
-	struct vehicle_vicon_position_s vicon_pos;
-	memset(&vicon_pos, 0, sizeof(vicon_pos));
-	struct actuator_controls_effective_s act_eff;
-	memset(&act_eff, 0, sizeof(act_eff));
-	struct vehicle_gps_position_s gps;
-	memset(&gps, 0, sizeof(gps));
-	struct vehicle_local_position_s local_pos_est;
-	memset(&local_pos_est, 0, sizeof(local_pos_est));
-	struct vehicle_global_position_s global_pos_est;
-	memset(&global_pos_est, 0, sizeof(global_pos_est));
-
-	/* subscribe */
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int sub_params = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0));
-	int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* advertise */
-	orb_advert_t local_pos_est_pub = 0;
-	orb_advert_t global_pos_est_pub = 0;
-
-	struct position_estimator_mc_params pos1D_params;
-	struct position_estimator_mc_param_handles pos1D_param_handles;
-	/* initialize parameter handles */
-	parameters_init(&pos1D_param_handles);
-
-	bool flag_use_gps = false;
-	bool flag_use_baro = false;
-	bool flag_baro_initialized = false; /* in any case disable baroINITdone */
-	/* FIRST PARAMETER READ at START UP*/
-	struct parameter_update_s update;
-	orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */
-	/* FIRST PARAMETER UPDATE */
-	parameters_update(&pos1D_param_handles, &pos1D_params);
-	flag_use_baro = pos1D_params.baro;
-	sigma = pos1D_params.sigma;
-	addNoise = pos1D_params.addNoise;
-	/* END FIRST PARAMETER UPDATE */
-
-	/* try to grab a vicon message - if it fails, go for GPS. */
-
-	/* make sure the next orb_check() can't return true unless we get a timely update */
-	orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
-	/* allow 200 ms for vicon to come in */
-	usleep(200000);
-	/* check if we got vicon */
-	bool update_check;
-	orb_check(vicon_pos_sub, &update_check);
-	/* if no update was available, use GPS */
-	flag_use_gps = !update_check;
-
-	if (flag_use_gps) {
-		mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked");
-		/* wait until gps signal turns valid, only then can we initialize the projection */
-
-		// XXX magic number
-		float hdop_threshold_m = 4.0f;
-		float vdop_threshold_m = 8.0f;
-
-		/*
-		 * If horizontal dilution of precision (hdop / eph)
-		 * and vertical diluation of precision (vdop / epv)
-		 * are below a certain threshold (e.g. 4 m), AND
-		 * home position is not yet set AND the last GPS
-		 * GPS measurement is not older than two seconds AND
-		 * the system is currently not armed, set home
-		 * position to the current position.
-		 */
-
-		while (!(gps.fix_type == 3
-			&& (gps.eph_m < hdop_threshold_m)
-			&& (gps.epv_m < vdop_threshold_m)
-			&& (hrt_absolute_time() - gps.timestamp_position < 2000000))) {
-
-			struct pollfd fds1[2] = {
-					{ .fd = vehicle_gps_sub, .events = POLLIN },
-					{ .fd = sub_params,   .events = POLLIN },
-			};
-
-			/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
-			 * this choice is critical, since the vehicle status might not
-			 * actually change, if this app is started after GPS lock was
-			 * aquired.
-			 */
-			if (poll(fds1, 2, 5000)) {
-				if (fds1[0].revents & POLLIN){
-					/* Read gps position */
-					orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-				}
-				if (fds1[1].revents & POLLIN){
-					/* Read out parameters to check for an update there, e.g. useGPS variable */
-					/* read from param to clear updated flag */
-					struct parameter_update_s updated;
-					orb_copy(ORB_ID(parameter_update), sub_params, &updated);
-					/* update parameters */
-					parameters_update(&pos1D_param_handles, &pos1D_params);
-				}
-			}
-			static int printcounter = 0;
-			if (printcounter == 100) {
-				printcounter = 0;
-				warnx("[pos_est_mc] wait for GPS fix");
-			}
-			printcounter++;
-		}
-
-		/* get gps value for first initialization */
-		orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-		lat_current = ((double)(gps.lat)) * 1e-7d;
-		lon_current = ((double)(gps.lon)) * 1e-7d;
-		alt_current = gps.alt * 1e-3f;
-		gps_origin_altitude = alt_current;
-		/* initialize coordinates */
-		map_projection_init(lat_current, lon_current);
-		/* publish global position messages only after first GPS message */
-		printf("[pos_est_mc] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
-
-	} else {
-		mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
-		/* onboard calculated position estimations */
-	}
-	thread_running = true;
-
-	struct pollfd fds2[3] = {
-		{ .fd = vehicle_gps_sub,   .events = POLLIN },
-		{ .fd = vicon_pos_sub,   .events = POLLIN },
-		{ .fd = sub_params,   .events = POLLIN },
-	};
-
-	bool vicon_updated = false;
-	bool gps_updated = false;
-
-	/**< main_loop */
-	while (!thread_should_exit) {
-		int ret = poll(fds2, 3, 20);  //wait maximal this 20 ms = 50 Hz minimum rate
-		if (ret < 0) {
-			/* poll error */
-		} else {
-			if (fds2[2].revents & POLLIN){
-				/* new parameter */
-				/* read from param to clear updated flag */
-				struct parameter_update_s updated;
-				orb_copy(ORB_ID(parameter_update), sub_params, &updated);
-				/* update parameters */
-				parameters_update(&pos1D_param_handles, &pos1D_params);
-				flag_use_baro = pos1D_params.baro;
-				sigma = pos1D_params.sigma;
-				addNoise = pos1D_params.addNoise;
-			}
-			vicon_updated = false; /* default is no vicon_updated */
-			if (fds2[1].revents & POLLIN) {
-				/* new vicon position */
-				orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
-				posX = vicon_pos.x;
-				posY = vicon_pos.y;
-				posZ = vicon_pos.z;
-				vicon_updated = true; /* set flag for vicon update */
-			} /* end of poll call for vicon updates */
-			gps_updated = false;
-			if (fds2[0].revents & POLLIN) {
-				/* new GPS value */
-				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-				/* Project gps lat lon (Geographic coordinate system) to plane*/
-				map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1]));
-				posX = z[0];
-				posY = z[1];
-				posZ = (float)(gps.alt * 1e-3f);
-				gps_updated = true;
-			}
-
-			/* Main estimator loop */
-			orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff);
-			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
-			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor);
-			// barometric pressure estimation at start up
-			if (!flag_baro_initialized){
-				// mean calculation over several measurements
-				if (baro_loop_cnt<baro_loop_end) {
-					p0_Pa += (sensor.baro_pres_mbar*100);
-					baro_loop_cnt++;
-				} else {
-					p0_Pa /= (float)(baro_loop_cnt);
-					flag_baro_initialized = true;
-					char *baro_m_start = "barometer initialized with p0 = ";
-					char p0_char[15];
-					sprintf(p0_char, "%8.2f", (double)(p0_Pa/100));
-					char *baro_m_end = " mbar";
-					char str[80];
-					strcpy(str,baro_m_start);
-					strcat(str,p0_char);
-					strcat(str,baro_m_end);
-					mavlink_log_info(mavlink_fd, str);
-				}
-			}
-			if (flag_use_gps) {
-				/* initialize map projection with the last estimate (not at full rate) */
-				if (gps.fix_type > 2) {
-					/* x-y-position/velocity estimation in earth frame = gps frame */
-					kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori);
-					memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
-					kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori);
-					memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
-					/* z-position/velocity estimation in earth frame = vicon frame */
-					float z_est = 0.0f;
-					if (flag_baro_initialized && flag_use_baro) {
-						z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity);
-						K[0] = K_vicon_50Hz[0];
-						K[1] = K_vicon_50Hz[1];
-						K[2] = K_vicon_50Hz[2];
-						gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */
-					} else {
-						z_est = posZ;
-						K[0] = K_vicon_50Hz[0];
-						K[1] = K_vicon_50Hz[1];
-						K[2] = K_vicon_50Hz[2];
-					}
-
-					kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori);
-					memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
-					local_pos_est.x = x_x_aposteriori_k[0];
-					local_pos_est.vx = x_x_aposteriori_k[1];
-					local_pos_est.y = x_y_aposteriori_k[0];
-					local_pos_est.vy = x_y_aposteriori_k[1];
-					local_pos_est.z = x_z_aposteriori_k[0];
-					local_pos_est.vz = x_z_aposteriori_k[1];
-					local_pos_est.timestamp = hrt_absolute_time();
-					if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) {
-						/* publish local position estimate */
-						if (local_pos_est_pub > 0) {
-							orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
-						} else {
-							local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
-						}
-						/* publish on GPS updates */
-						if (gps_updated) {
-
-							double lat, lon;
-							float alt = z_est + gps_origin_altitude;
-
-							map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon);
-
-							global_pos_est.lat = lat;
-							global_pos_est.lon = lon;
-							global_pos_est.alt = alt;
-
-							if (global_pos_est_pub > 0) {
-								orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est);
-							} else {
-								global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est);
-							}
-						}
-					}
-				}
-			} else {
-				/* x-y-position/velocity estimation in earth frame = vicon frame */
-				kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori);
-				memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
-				kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori);
-				memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
-				/* z-position/velocity estimation in earth frame = vicon frame */
-				float z_est = 0.0f;
-				float local_sigma = 0.0f;
-				if (flag_baro_initialized && flag_use_baro) {
-					z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity);
-					K[0] = K_vicon_50Hz[0];
-					K[1] = K_vicon_50Hz[1];
-					K[2] = K_vicon_50Hz[2];
-					vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */
-					local_sigma = 0.0f; /* don't add noise on barometer in any case */
-				} else {
-					z_est = posZ;
-					K[0] = K_vicon_50Hz[0];
-					K[1] = K_vicon_50Hz[1];
-					K[2] = K_vicon_50Hz[2];
-					local_sigma = sigma;
-				}
-				kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori);
-				memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
-				local_pos_est.x = x_x_aposteriori_k[0];
-				local_pos_est.vx = x_x_aposteriori_k[1];
-				local_pos_est.y = x_y_aposteriori_k[0];
-				local_pos_est.vy = x_y_aposteriori_k[1];
-				local_pos_est.z = x_z_aposteriori_k[0];
-				local_pos_est.vz = x_z_aposteriori_k[1];
-				local_pos_est.timestamp = hrt_absolute_time();
-				if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
-					if(local_pos_est_pub > 0)
-						orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
-					else
-						local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
-					//char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]);
-					//mavlink_log_info(mavlink_fd, buf);
-				}
-			}
-		} /* end of poll return value check */
-	}
-
-	printf("[pos_est_mc] exiting.\n");
-	mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting");
-	thread_running = false;
-	return 0;
-}
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.c b/src/modules/position_estimator_mc/position_estimator_mc_params.c
deleted file mode 100755
index 72e5bc73b..000000000
--- a/src/modules/position_estimator_mc/position_estimator_mc_params.c
+++ /dev/null
@@ -1,68 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: 	Damian Aregger <daregger@student.ethz.ch>
- *   			Tobias Naegeli <naegelit@student.ethz.ch>
-* 				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
- * 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 position_estimator_mc_params.c
- * 
- * Parameters for position_estimator_mc
- */
-
-#include "position_estimator_mc_params.h"
-
-/* Kalman Filter covariances */
-/* gps measurement noise standard deviation */
-PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f);
-PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f);
-PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f);
-PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f);
-
-int parameters_init(struct position_estimator_mc_param_handles *h)
-{
-	h->addNoise	= param_find("POS_EST_ADDN");
-	h->sigma = param_find("POS_EST_SIGMA");
-	h->r = param_find("POS_EST_R");
-	h->baro_param_handle = param_find("POS_EST_BARO");
-	return OK;
-}
-
-int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p)
-{
-	param_get(h->addNoise, &(p->addNoise));
-	param_get(h->sigma, &(p->sigma));
-	param_get(h->r, &(p->R));
-	param_get(h->baro_param_handle, &(p->baro));
-	return OK;
-}
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index fffc6b347..f86ed3a6e 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -47,8 +47,8 @@
 #include "sbus.h"
 
 #define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
-#define RC_CHANNEL_HIGH_THRESH		5000
-#define RC_CHANNEL_LOW_THRESH		-5000
+#define RC_CHANNEL_HIGH_THRESH		5000	/* 75% threshold */
+#define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
 
@@ -134,8 +134,6 @@ controls_tick() {
 
 	perf_begin(c_gather_sbus);
 
-	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
-
 	bool sbus_failsafe, sbus_frame_drop;
 
 	/* 
@@ -208,94 +206,105 @@ controls_tick() {
 		/* update RC-received timestamp */
 		system_state.rc_channels_timestamp_received = hrt_absolute_time();
 
-		/* do not command anything in failsafe, kick in the RC loss counter */
-		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
-
-			/* update RC-received timestamp */
-			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
-
-			/* map raw inputs to mapped inputs */
-			/* XXX mapping should be atomic relative to protocol */
-			for (unsigned i = 0; i < r_raw_rc_count; i++) {
-
-				/* map the input channel */
-				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
-
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
-
-					uint16_t raw = r_raw_rc_values[i];
-
-					int16_t scaled;
-
-					/*
-					 * 1) Constrain to min/max values, as later processing depends on bounds.
-					 */
-					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
-						raw = conf[PX4IO_P_RC_CONFIG_MIN];
-					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
-						raw = conf[PX4IO_P_RC_CONFIG_MAX];
-
-					/*
-					 * 2) Scale around the mid point differently for lower and upper range.
-					 *
-					 * This is necessary as they don't share the same endpoints and slope.
-					 *
-					 * First normalize to 0..1 range with correct sign (below or above center),
-					 * then scale to 20000 range (if center is an actual center, -10000..10000,
-					 * if parameters only support half range, scale to 10000 range, e.g. if
-					 * center == min 0..10000, if center == max -10000..0).
-					 *
-					 * As the min and max bounds were enforced in step 1), division by zero
-					 * cannot occur, as for the case of center == min or center == max the if
-					 * statement is mutually exclusive with the arithmetic NaN case.
-					 *
-					 * DO NOT REMOVE OR ALTER STEP 1!
-					 */
-					if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
-
-					} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
-
-					} else {
-						/* in the configured dead zone, output zero */
-						scaled = 0;
-					}
-
-					/* invert channel if requested */
-					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-						scaled = -scaled;
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
+
+		/* map raw inputs to mapped inputs */
+		/* XXX mapping should be atomic relative to protocol */
+		for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+			/* map the input channel */
+			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+
+			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+
+				uint16_t raw = r_raw_rc_values[i];
+
+				int16_t scaled;
+
+				/*
+				 * 1) Constrain to min/max values, as later processing depends on bounds.
+				 */
+				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+					raw = conf[PX4IO_P_RC_CONFIG_MIN];
+				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+
+				/*
+				 * 2) Scale around the mid point differently for lower and upper range.
+				 *
+				 * This is necessary as they don't share the same endpoints and slope.
+				 *
+				 * First normalize to 0..1 range with correct sign (below or above center),
+				 * then scale to 20000 range (if center is an actual center, -10000..10000,
+				 * if parameters only support half range, scale to 10000 range, e.g. if
+				 * center == min 0..10000, if center == max -10000..0).
+				 *
+				 * As the min and max bounds were enforced in step 1), division by zero
+				 * cannot occur, as for the case of center == min or center == max the if
+				 * statement is mutually exclusive with the arithmetic NaN case.
+				 *
+				 * DO NOT REMOVE OR ALTER STEP 1!
+				 */
+				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+
+				} else {
+					/* in the configured dead zone, output zero */
+					scaled = 0;
+				}
 
-					/* and update the scaled/mapped version */
-					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-					if (mapped < PX4IO_CONTROL_CHANNELS) {
+				/* invert channel if requested */
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
+					scaled = -scaled;
+				}
 
-						/* invert channel if pitch - pulling the lever down means pitching up by convention */
-						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
-							scaled = -scaled;
+				/* and update the scaled/mapped version */
+				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
-						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-						assigned_channels |= (1 << mapped);
+					/* invert channel if pitch - pulling the lever down means pitching up by convention */
+					if (mapped == 1) {
+						/* roll, pitch, yaw, throttle, override is the standard order */
+						scaled = -scaled;
+					}
 
+					if (mapped == 3 && r_setup_rc_thr_failsafe) {
+						/* throttle failsafe detection */
+						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
+						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
+							r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+						} else {
+							r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+						}
 					}
+
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
+
 				}
 			}
+		}
 
-			/* set un-assigned controls to zero */
-			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
-				if (!(assigned_channels & (1 << i)))
-					r_rc_values[i] = 0;
+		/* set un-assigned controls to zero */
+		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+			if (!(assigned_channels & (1 << i))) {
+				r_rc_values[i] = 0;
 			}
+		}
 
-			/* set RC OK flag, as we got an update */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+		/* set RC OK flag, as we got an update */
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+		r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK;
 
-			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
-			if (assigned_channels > 4) {
-				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
-			} else {
-				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
-			}
+		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
+		if (assigned_channels > 4) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
 		}
 
 		/*
@@ -323,37 +332,42 @@ controls_tick() {
 	 * Handle losing RC input
 	 */
 
-	/* this kicks in if the receiver is gone or the system went to failsafe */
-	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+	/* if we are in failsafe, clear the override flag */
+	if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
+	}
+
+	/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
+	if (rc_input_lost) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
 			PX4IO_P_STATUS_FLAGS_RC_OK);
 
+		/* flag raw RC as lost */
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
+
 		/* Mark all channels as invalid, as we just lost the RX */
 		r_rc_valid = 0;
 
+		/* Set raw channel count to zero */
+		r_raw_rc_count = 0;
+
 		/* Set the RC_LOST alarm */
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
 	}
 
-	/* this kicks in if the receiver is completely gone */
-	if (rc_input_lost) {
-
-		/* Set channel count to zero */
-		r_raw_rc_count = 0;
-	}
-
 	/*
 	 * Check for manual override.
 	 *
 	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
-	 * must have R/C input.
+	 * must have R/C input (NO FAILSAFE!).
 	 * Override is enabled if either the hardcoded channel / value combination
 	 * is selected, or the AP has requested it.
 	 */
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 
 		bool override = false;
 
@@ -376,10 +390,10 @@ controls_tick() {
 				mixer_tick();
 
 		} else {
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+			r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 		}
 	} else {
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 	}
 }
 
@@ -402,8 +416,9 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
 			*num_values = PX4IO_RC_INPUT_CHANNELS;
 
-		for (unsigned i = 0; i < *num_values; i++)
+		for (unsigned i = 0; i < *num_values; i++) {
 			values[i] = ppm_buffer[i];
+		}
 
 		/* clear validity */
 		ppm_last_valid_decode = 0;
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 79b6546b3..6d1d1fc2d 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -64,12 +64,15 @@
 #define rCCR		REG(STM32_I2C_CCR_OFFSET)
 #define rTRISE		REG(STM32_I2C_TRISE_OFFSET)
 
+void			i2c_reset(void);
 static int		i2c_interrupt(int irq, void *context);
 static void		i2c_rx_setup(void);
 static void		i2c_tx_setup(void);
 static void		i2c_rx_complete(void);
 static void		i2c_tx_complete(void);
+#ifdef DEBUG
 static void		i2c_dump(void);
+#endif
 
 static DMA_HANDLE	rx_dma;
 static DMA_HANDLE	tx_dma;
@@ -331,6 +334,7 @@ i2c_tx_complete(void)
 	i2c_tx_setup();
 }
 
+#ifdef DEBUG
 static void
 i2c_dump(void)
 {
@@ -339,3 +343,4 @@ i2c_dump(void)
 	debug("CCR   0x%08x  TRISE 0x%08x", rCCR,  rTRISE);
 	debug("SR1   0x%08x  SR2   0x%08x", rSR1,  rSR2);
 }
+#endif
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f6a54c5ed..8f47a9efb 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -224,6 +224,7 @@ mixer_tick(void)
 		actuator_count = mixed;
 		in_mixer = false;
 
+		/* the pwm limit call takes care of out of band errors */
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
@@ -265,14 +266,12 @@ mixer_tick(void)
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
 
-		/* update S.BUS1 outputs */
-		if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
-			sbus1_output(r_page_servos, actuator_count);
-		}
+		/* set S.BUS1 or S.BUS2 outputs */
 
-		/* update S.BUS2 outputs */
-		if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
-			sbus2_output(r_page_servos, actuator_count);
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
+			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
+		} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
+			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
 		}
 
 	} else if (mixer_servos_armed && should_always_enable_pwm) {
@@ -280,15 +279,12 @@ mixer_tick(void)
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
 
-		/* update S.BUS1 outputs */
-		if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
-			sbus1_output(r_page_servos, actuator_count);
-		}
+		/* set S.BUS1 or S.BUS2 outputs */
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
+			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
 
-		/* update S.BUS2 outputs */
-		if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
-			sbus2_output(r_page_servos, actuator_count);
-		}
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
+			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
 	}
 }
 
@@ -298,7 +294,7 @@ mixer_callback(uintptr_t handle,
 	       uint8_t control_index,
 	       float &control)
 {
-	if (control_group > 3)
+	if (control_group >= PX4IO_CONTROL_GROUPS)
 		return -1;
 
 	switch (source) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d48c6c529..d5a6b1ec4 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -142,6 +142,7 @@
 #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE		(1 << 1) /* receiver is in failsafe mode */
 #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11		(1 << 2) /* DSM decoding is 11 bit mode */
 #define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK		(1 << 3) /* Channel mapping is ok */
+#define PX4IO_P_RAW_RC_FLAGS_RC_OK		(1 << 4) /* RC reception ok */
 
 #define PX4IO_P_RAW_RC_NRSSI			2	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
 #define PX4IO_P_RAW_RC_DATA			3	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
@@ -164,10 +165,10 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
-#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /* enable S.Bus v1 output */
-#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
-#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
-#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /**< enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /**< enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /**< enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /**< enable ADC RSSI parsing */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -189,6 +190,8 @@
 #define PX4IO_P_SETUP_RELAYS_POWER2		(1<<1)	/* hardware rev [1] power relay 2 */
 #define PX4IO_P_SETUP_RELAYS_ACC1		(1<<2)	/* hardware rev [1] accessory power 1 */
 #define PX4IO_P_SETUP_RELAYS_ACC2		(1<<3)	/* hardware rev [1] accessory power 2 */
+#else
+#define PX4IO_P_SETUP_RELAYS_PAD		5
 #endif
 
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* hardware rev [1] battery voltage correction factor (float) */
@@ -201,26 +204,33 @@ enum {							/* DSM bind states */
 	dsm_bind_send_pulses,
 	dsm_bind_reinit_uart
 };
- 					     /*	8 */
-#define PX4IO_P_SETUP_SET_DEBUG			9		/* debug level for IO board */
+						/* 8 */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
-#define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
-#define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
+#define PX4IO_P_SETUP_REBOOT_BL			10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC			14662	/* required argument for reboot (random) */
 
-#define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
+#define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
+						/* storage space of 12 occupied by CRC */
+#define PX4IO_P_SETUP_FORCE_SAFETY_OFF		12	/* force safety switch into
+                                                           'armed' (PWM enabled) state - this is a non-data write and
+                                                           hence index 12 can safely be used. */
+#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
+
+#define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
+#define PX4IO_PAGE_CONTROLS			51	/**< actuator control groups, one after the other, 8 wide */
 #define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 #define PX4IO_P_CONTROLS_GROUP_1		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 #define PX4IO_P_CONTROLS_GROUP_2		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 #define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 
 #define PX4IO_P_CONTROLS_GROUP_VALID		64
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /* group 0 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /* group 1 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /* group 2 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /* group 3 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /**< group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /**< group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /**< group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /**< group 3 is valid / received */
 
 /* raw text load to the mixer parser - ignores offset */
 #define PX4IO_PAGE_MIXERLOAD			52
@@ -302,7 +312,7 @@ struct IOPacket {
 
 #define PKT_COUNT(_p)	((_p).count_code & PKT_COUNT_MASK)
 #define PKT_CODE(_p)	((_p).count_code & PKT_CODE_MASK)
-#define PKT_SIZE(_p)	((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
+#define PKT_SIZE(_p)	((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
 
 static const uint8_t crc8_tab[256] __attribute__((unused)) =
 {
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index d4c25911e..cd134ccb4 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -37,6 +37,7 @@
  */
 
 #include <nuttx/config.h>
+#include <nuttx/arch.h>
 
 #include <stdio.h>	// required for task_create
 #include <stdbool.h>
@@ -303,14 +304,12 @@ user_start(int argc, char *argv[])
 		 */
 		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
 
-			struct mallinfo minfo = mallinfo();
-
 			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", 
 				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
 				  (unsigned)r_status_flags,
 				  (unsigned)r_setup_arming,
 				  (unsigned)r_setup_features,
-				  (unsigned)minfo.mxordblk);
+				  (unsigned)mallinfo().mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}
 	}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 39df81eed..2ae081d51 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -56,7 +56,7 @@
  */
 #define PX4IO_SERVO_COUNT		8
 #define PX4IO_CONTROL_CHANNELS		8
-#define PX4IO_CONTROL_GROUPS		2
+#define PX4IO_CONTROL_GROUPS		4
 #define PX4IO_RC_INPUT_CHANNELS		18
 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS		8 /**< This is the maximum number of channels mapped/used */
 
@@ -111,6 +111,7 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
 #endif
+#define r_setup_rc_thr_failsafe	r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
 
 #define r_control_values	(&r_page_controls[0])
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 97d25bbfa..b37259997 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -119,7 +119,6 @@ uint16_t		r_page_raw_rc_input[] =
 	[PX4IO_P_RAW_RC_DATA]			= 0,
 	[PX4IO_P_RAW_FRAME_COUNT]		= 0,
 	[PX4IO_P_RAW_LOST_FRAME_COUNT]		= 0,
-	[PX4IO_P_RAW_RC_DATA]			= 0,
 	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
 };
 
@@ -160,6 +159,9 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#else
+	/* this is unused, but we will pad it for readability (the compiler pads it automatically) */
+	[PX4IO_P_SETUP_RELAYS_PAD]		= 0,
 #endif
 #ifdef ADC_VSERVO
 	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
@@ -169,6 +171,7 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
+	[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
 };
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
@@ -462,9 +465,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			#ifdef ENABLE_SBUS_OUT
 			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
 
-			/* disable the conflicting options */
-			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
-				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+			/* disable the conflicting options with SBUS 1 */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* disable the conflicting options with SBUS 2 */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
 			}
 			#endif
 
@@ -513,18 +525,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
-			if (value < 50)
+			if (value < 50) {
 				value = 50;
-			if (value > 400)
+			}
+			if (value > 400) {
 				value = 400;
+			}
 			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
 			break;
 
 		case PX4IO_P_SETUP_PWM_ALTRATE:
-			if (value < 50)
+			if (value < 50) {
 				value = 50;
-			if (value > 400)
+			}
+			if (value > 400) {
 				value = 400;
+			}
 			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
 			break;
 
@@ -556,8 +572,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			}
 
 			// check the magic value
-			if (value != PX4IO_REBOOT_BL_MAGIC)
+			if (value != PX4IO_REBOOT_BL_MAGIC) {
 				break;
+			}
 
                         // we schedule a reboot rather than rebooting
                         // immediately to allow the IO board to ACK
@@ -569,6 +586,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;
 
+		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
+			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+			}
+			break;
+
+		case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
+			if (value > 650 && value < 2350) {
+				r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
+			}
+			break;
+
 		default:
 			return -1;
 		}
@@ -640,7 +669,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
 					disabled = true;
-				} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
+				} else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
 					count++;
 				}
 
@@ -682,7 +711,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 {
 #define SELECT_PAGE(_page_name)							\
 	do {									\
-		*values = &_page_name[0];					\
+		*values = (uint16_t *)&_page_name[0];				\
 		*num_values = sizeof(_page_name) / sizeof(_page_name[0]);	\
 	} while(0)
 
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 33ef515be..925e24f65 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -109,7 +109,7 @@ int
 sbus_init(const char *device)
 {
 	if (sbus_fd < 0)
-		sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
+		sbus_fd = open(device, O_RDWR | O_NONBLOCK);
 
 	if (sbus_fd >= 0) {
 		struct termios t;
@@ -138,21 +138,15 @@ sbus_init(const char *device)
 void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
-	/*
-	 * S.BUS2 outputs are defined as:
-	 *
-	 */
-	 #warning SBUS1 output is not yet implemented
+	char a = 'A';
+	write(sbus_fd, &a, 1);
 }
 
 void
 sbus2_output(uint16_t *values, uint16_t num_values)
 {
-	/*
-	 * S.BUS2 outputs are defined as:
-	 *
-	 */
-	#warning SBUS2 output is not yet implemented
+	char b = 'B';
+	write(sbus_fd, &b, 1);
 }
 
 bool
@@ -280,6 +274,7 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v
 						case 0x03:
 							{
 								uint16_t rx_voltage = (frame[1] << 8) | frame[2];
+								isr_debug(30, "rx_voltage %d", (int)rx_voltage);
 								n_consumed = 3;
 							}
 							break;
@@ -308,6 +303,7 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v
 						case 0x13:
 							{
 								uint16_t gps_something = (frame[1] << 8) | frame[2];
+								isr_debug(30, "gps_something %d", (int)gps_something);
 								n_consumed = 24;
 							}
 							break;
diff --git a/src/modules/sdlog/module.mk b/src/modules/sdlog/module.mk
deleted file mode 100644
index 89da2d827..000000000
--- a/src/modules/sdlog/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 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.
-#
-############################################################################
-
-#
-# sdlog Application
-#
-
-MODULE_COMMAND	 = sdlog
-# The main thread only buffers to RAM, needs a high priority
-MODULE_PRIORITY	 = "SCHED_PRIORITY_MAX-30"
-
-SRCS		 = sdlog.c \
-		   sdlog_ringbuffer.c
diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c
deleted file mode 100644
index c22523bf2..000000000
--- a/src/modules/sdlog/sdlog.c
+++ /dev/null
@@ -1,840 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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
- * 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 sdlog.c
- * @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Simple SD logger for flight data. Buffers new sensor values and
- * does the heavy SD I/O in a low-priority worker thread.
- */
-
-#include <nuttx/config.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <sys/prctl.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <poll.h>
-#include <stdlib.h>
-#include <string.h>
-#include <ctype.h>
-#include <systemlib/err.h>
-#include <unistd.h>
-#include <drivers/drv_hrt.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/airspeed.h>
-
-#include <systemlib/systemlib.h>
-
-#include <mavlink/mavlink_log.h>
-
-#include "sdlog_ringbuffer.h"
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log folders */
-
-static const char *mountpoint = "/fs/microsd";
-static const char *mfile_in = "/etc/logging/logconv.m";
-int sysvector_file = -1;
-int mavlink_fd = -1;
-struct sdlog_logbuffer lb;
-
-/* mutex / condition to synchronize threads */
-pthread_mutex_t sysvector_mutex;
-pthread_cond_t sysvector_cond;
-
-/**
- * System state vector log buffer writing
- */
-static void *sdlog_sysvector_write_thread(void *arg);
-
-/**
- * Create the thread to write the system vector
- */
-pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf);
-
-/**
- * SD log management function.
- */
-__EXPORT int sdlog_main(int argc, char *argv[]);
-
-/**
- * Mainloop of sd log deamon.
- */
-int sdlog_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static int file_exist(const char *filename);
-
-static int file_copy(const char *file_old, const char *file_new);
-
-static void handle_command(struct vehicle_command_s *cmd);
-
-/**
- * Print the current status.
- */
-static void print_sdlog_status(void);
-
-/**
- * Create folder for current logging session.
- */
-static int create_logfolder(char *folder_path);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n");
-}
-
-// XXX turn this into a C++ class
-unsigned sensor_combined_bytes = 0;
-unsigned actuator_outputs_bytes = 0;
-unsigned actuator_controls_bytes = 0;
-unsigned sysvector_bytes = 0;
-unsigned blackbox_file_bytes = 0;
-uint64_t starttime = 0;
-
-/* logging on or off, default to true */
-bool logging_enabled = true;
-
-/**
- * The sd log deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_spawn_cmd().
- */
-int sdlog_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("sdlog already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("sdlog",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_DEFAULT - 30,
-					 4096,
-					 sdlog_thread_main,
-					 (const char **)argv);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		if (!thread_running) {
-			printf("\tsdlog is not started\n");
-		}
-
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			print_sdlog_status();
-
-		} else {
-			printf("\tsdlog not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int create_logfolder(char *folder_path)
-{
-	/* make folder on sdcard */
-	uint16_t foldernumber = 1; // start with folder 0001
-	int mkdir_ret;
-
-	/* look for the next folder that does not exist */
-	while (foldernumber < MAX_NO_LOGFOLDER) {
-		/* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */
-		sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber);
-		mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO);
-		/* the result is -1 if the folder exists */
-
-		if (mkdir_ret == 0) {
-			/* folder does not exist, success */
-
-			/* now copy the Matlab/Octave file */
-			char mfile_out[100];
-			sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber);
-			int ret = file_copy(mfile_in, mfile_out);
-
-			if (!ret) {
-				warnx("copied m file to %s", mfile_out);
-
-			} else {
-				warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out);
-			}
-
-			break;
-
-		} else if (mkdir_ret == -1) {
-			/* folder exists already */
-			foldernumber++;
-			continue;
-
-		} else {
-			warn("failed creating new folder");
-			return -1;
-		}
-	}
-
-	if (foldernumber >= MAX_NO_LOGFOLDER) {
-		/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
-		warn("all %d possible folders exist already", MAX_NO_LOGFOLDER);
-		return -1;
-	}
-
-	return 0;
-}
-
-
-static void *
-sdlog_sysvector_write_thread(void *arg)
-{
-	/* set name */
-	prctl(PR_SET_NAME, "sdlog microSD I/O", 0);
-
-	struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg;
-
-	int poll_count = 0;
-	struct sdlog_sysvector sysvect;
-	memset(&sysvect, 0, sizeof(sysvect));
-
-	while (!thread_should_exit) {
-		
-		/* make sure threads are synchronized */
-		pthread_mutex_lock(&sysvector_mutex);
-
-		/* only wait if no data is available to process */
-		if (sdlog_logbuffer_is_empty(logbuf)) {
-			/* blocking wait for new data at this line */
-			pthread_cond_wait(&sysvector_cond, &sysvector_mutex);
-		}
-
-		/* only quickly load data, do heavy I/O a few lines down */
-		int ret = sdlog_logbuffer_read(logbuf, &sysvect);
-		/* continue */
-		pthread_mutex_unlock(&sysvector_mutex);
-
-		if (ret == OK) {
-			sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect));
-		}
-
-		if (poll_count % 100 == 0) {
-			fsync(sysvector_file);
-		}
-
-		poll_count++;
-	}
-
-	fsync(sysvector_file);
-
-	return OK;
-}
-
-pthread_t
-sysvector_write_start(struct sdlog_logbuffer *logbuf)
-{
-	pthread_attr_t receiveloop_attr;
-	pthread_attr_init(&receiveloop_attr);
-
-	struct sched_param param;
-	/* low priority, as this is expensive disk I/O */
-	param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
-	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
-
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf);
-	return thread;
-
-	// XXX we have to destroy the attr at some point
-}
-
-
-int sdlog_thread_main(int argc, char *argv[])
-{
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
-	}
-
-	/* log every n'th value (skip three per default) */
-	int skip_value = 3;
-
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-	int ch;
-
-	while ((ch = getopt(argc, argv, "s:r")) != EOF) {
-		switch (ch) {
-		case 's':
-			{
-			/* log only every n'th (gyro clocked) value */
-			unsigned s = strtoul(optarg, NULL, 10);
-
-			if (s < 1 || s > 250) {
-				errx(1, "Wrong skip value of %d, out of range (1..250)\n", s);
-			} else {
-				skip_value = s;
-			}
-			}
-			break;
-
-		case 'r':
-			/* log only on request, disable logging per default */
-			logging_enabled = false;
-			break;
-
-		case '?':
-			if (optopt == 'c') {
-				warnx("Option -%c requires an argument.\n", optopt);
-			} else if (isprint(optopt)) {
-				warnx("Unknown option `-%c'.\n", optopt);
-			} else {
-				warnx("Unknown option character `\\x%x'.\n", optopt);
-			}
-
-		default:
-			usage("unrecognized flag");
-			errx(1, "exiting.");
-		}
-	}
-
-	if (file_exist(mountpoint) != OK) {
-		errx(1, "logging mount point %s not present, exiting.", mountpoint);
-	}
-
-	char folder_path[64];
-
-	if (create_logfolder(folder_path))
-		errx(1, "unable to create logging folder, exiting.");
-
-	FILE *gpsfile;
-	FILE *blackbox_file;
-
-	/* string to hold the path to the sensorfile */
-	char path_buf[64] = "";
-
-	/* only print logging path, important to find log file later */
-	warnx("logging to directory %s\n", folder_path);
-
-	/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
-	sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector");
-
-	if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
-		errx(1, "opening %s failed.\n", path_buf);
-	}
-
-	/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
-	sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
-
-	if (NULL == (gpsfile = fopen(path_buf, "w"))) {
-		errx(1, "opening %s failed.\n", path_buf);
-	}
-
-	int gpsfile_no = fileno(gpsfile);
-
-	/* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */
-	sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox");
-
-	if (NULL == (blackbox_file = fopen(path_buf, "w"))) {
-		errx(1, "opening %s failed.\n", path_buf);
-	}
-
-	// XXX for fsync() calls
-	int blackbox_file_no = fileno(blackbox_file);
-
-	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 25;
-	/* Sanity check variable and index */
-	ssize_t fdsc_count = 0;
-	/* file descriptors to wait for */
-	struct pollfd fds[fdsc];
-
-
-	struct {
-		struct sensor_combined_s raw;
-		struct vehicle_attitude_s att;
-		struct vehicle_attitude_setpoint_s att_sp;
-		struct actuator_outputs_s act_outputs;
-		struct actuator_controls_s act_controls;
-		struct actuator_controls_effective_s act_controls_effective;
-		struct vehicle_command_s cmd;
-		struct vehicle_local_position_s local_pos;
-		struct vehicle_global_position_s global_pos;
-		struct vehicle_gps_position_s gps_pos;
-		struct vehicle_vicon_position_s vicon_pos;
-		struct optical_flow_s flow;
-		struct battery_status_s batt;
-		struct differential_pressure_s diff_pres;
-		struct airspeed_s airspeed;
-	} buf;
-	memset(&buf, 0, sizeof(buf));
-
-	struct {
-		int cmd_sub;
-		int sensor_sub;
-		int att_sub;
-		int spa_sub;
-		int act_0_sub;
-		int controls_0_sub;
-		int controls_effective_0_sub;
-		int local_pos_sub;
-		int global_pos_sub;
-		int gps_pos_sub;
-		int vicon_pos_sub;
-		int flow_sub;
-		int batt_sub;
-		int diff_pres_sub;
-		int airspeed_sub;
-	} subs;
-
-	/* --- MANAGEMENT - LOGGING COMMAND --- */
-	/* subscribe to ORB for vehicle command */
-	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	fds[fdsc_count].fd = subs.cmd_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GPS POSITION --- */
-	/* subscribe to ORB for global position */
-	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	fds[fdsc_count].fd = subs.gps_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- SENSORS RAW VALUE --- */
-	/* subscribe to ORB for sensors raw */
-	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	fds[fdsc_count].fd = subs.sensor_sub;
-	/* do not rate limit, instead use skip counter (aliasing on rate limit) */
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ATTITUDE VALUE --- */
-	/* subscribe to ORB for attitude */
-	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	fds[fdsc_count].fd = subs.att_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ATTITUDE SETPOINT VALUE --- */
-	/* subscribe to ORB for attitude setpoint */
-	/* struct already allocated */
-	subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	fds[fdsc_count].fd = subs.spa_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/** --- ACTUATOR OUTPUTS --- */
-	subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
-	fds[fdsc_count].fd = subs.act_0_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ACTUATOR CONTROL VALUE --- */
-	/* subscribe to ORB for actuator control */
-	subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	fds[fdsc_count].fd = subs.controls_0_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */
-	/* subscribe to ORB for actuator control */
-	subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
-	fds[fdsc_count].fd = subs.controls_effective_0_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- LOCAL POSITION --- */
-	/* subscribe to ORB for local position */
-	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	fds[fdsc_count].fd = subs.local_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GLOBAL POSITION --- */
-	/* subscribe to ORB for global position */
-	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	fds[fdsc_count].fd = subs.global_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- VICON POSITION --- */
-	/* subscribe to ORB for vicon position */
-	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	fds[fdsc_count].fd = subs.vicon_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- FLOW measurements --- */
-	/* subscribe to ORB for flow measurements */
-	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
-	fds[fdsc_count].fd = subs.flow_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- BATTERY STATUS --- */
-	/* subscribe to ORB for flow measurements */
-	subs.batt_sub = orb_subscribe(ORB_ID(battery_status));
-	fds[fdsc_count].fd = subs.batt_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- DIFFERENTIAL PRESSURE --- */
-	/* subscribe to ORB for flow measurements */
-	subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	fds[fdsc_count].fd = subs.diff_pres_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- AIRSPEED --- */
-	/* subscribe to ORB for airspeed */
-	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	fds[fdsc_count].fd = subs.airspeed_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;	
-
-	/* WARNING: If you get the error message below,
-	 * then the number of registered messages (fdsc)
-	 * differs from the number of messages in the above list.
-	 */
-	if (fdsc_count > fdsc) {
-		warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__);
-		fdsc_count = fdsc;
-	}
-
-	/*
-	 * set up poll to block for new data,
-	 * wait for a maximum of 1000 ms (1 second)
-	 */
-	// const int timeout = 1000;
-
-	thread_running = true;
-
-	/* initialize log buffer with a size of 10 */
-	sdlog_logbuffer_init(&lb, 10);
-
-	/* initialize thread synchronization */
-	pthread_mutex_init(&sysvector_mutex, NULL);
-  	pthread_cond_init(&sysvector_cond, NULL);
-
-	/* start logbuffer emptying thread */
-	pthread_t sysvector_pthread = sysvector_write_start(&lb);
-
-	starttime = hrt_absolute_time();
-
-	/* track skipping */
-	int skip_count = 0;
-
-	while (!thread_should_exit) {
-
-		/* only poll for commands, gps and sensor_combined */
-		int poll_ret = poll(fds, 3, 1000);
-
-		/* handle the poll result */
-		if (poll_ret == 0) {
-			/* XXX this means none of our providers is giving us data - might be an error? */
-		} else if (poll_ret < 0) {
-			/* XXX this is seriously bad - should be an emergency */
-		} else {
-
-			int ifds = 0;
-
-			/* --- VEHICLE COMMAND VALUE --- */
-			if (fds[ifds++].revents & POLLIN) {
-				/* copy command into local buffer */
-				orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
-
-				/* always log to blackbox, even when logging disabled */
-				blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
-					buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
-					(double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
-
-				handle_command(&buf.cmd);
-			}
-
-			/* --- VEHICLE GPS VALUE --- */
-			if (fds[ifds++].revents & POLLIN) {
-				/* copy gps position into local buffer */
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
-
-				/* if logging disabled, continue */
-				if (logging_enabled) {
-
-				/* write KML line */
-				}
-			}
-
-			/* --- SENSORS RAW VALUE --- */
-			if (fds[ifds++].revents & POLLIN) {
-
-				// /* copy sensors raw data into local buffer */
-				// orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
-				// /* write out */
-				// sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
-
-				/* always copy sensors raw data into local buffer, since poll flags won't clear else */
-				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
-				orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
-				orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
-				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
-				orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
-				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
-				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
-				orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres);
-				orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
-				orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
-
-				/* if skipping is on or logging is disabled, ignore */
-				if (skip_count < skip_value || !logging_enabled) {
-					skip_count++;
-					/* do not log data */
-					continue;
-				} else {
-					/* log data, reset */
-					skip_count = 0;
-				}
-
-				struct sdlog_sysvector sysvect = {
-					.timestamp = buf.raw.timestamp,
-					.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
-					.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
-					.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
-					.baro = buf.raw.baro_pres_mbar,
-					.baro_alt = buf.raw.baro_alt_meter,
-					.baro_temp = buf.raw.baro_temp_celcius,
-					.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
-					.actuators = {
-						buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
-						buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]
-					},
-					.vbat = buf.batt.voltage_v,
-					.bat_current = buf.batt.current_a,
-					.bat_discharged = buf.batt.discharged_mah,
-					.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
-					.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
-					.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
-					.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
-					.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
-					.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
-					.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
-					.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
-					.diff_pressure = buf.diff_pres.differential_pressure_pa,
-					.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
-					.true_airspeed = buf.airspeed.true_airspeed_m_s
-				};
-
-				/* put into buffer for later IO */
-				pthread_mutex_lock(&sysvector_mutex);
-				sdlog_logbuffer_write(&lb, &sysvect);
-				/* signal the other thread new data, but not yet unlock */
-				if ((unsigned)lb.count > (lb.size / 2)) {
-					/* only request write if several packets can be written at once */
-					pthread_cond_signal(&sysvector_cond);
-				}
-				/* unlock, now the writer thread may run */
-				pthread_mutex_unlock(&sysvector_mutex);
-			}
-
-		}
-
-	}
-
-	print_sdlog_status();
-
-	/* wake up write thread one last time */
-	pthread_mutex_lock(&sysvector_mutex);
-	pthread_cond_signal(&sysvector_cond);
-	/* unlock, now the writer thread may return */
-	pthread_mutex_unlock(&sysvector_mutex);
-
-	/* wait for write thread to return */
-	(void)pthread_join(sysvector_pthread, NULL);
-
-  	pthread_mutex_destroy(&sysvector_mutex);
-  	pthread_cond_destroy(&sysvector_cond);
-
-	warnx("exiting.\n\n");
-
-	/* finish KML file */
-	// XXX
-	fclose(gpsfile);
-	fclose(blackbox_file);
-
-	thread_running = false;
-
-	return 0;
-}
-
-void print_sdlog_status()
-{
-	unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
-	float mebibytes = bytes / 1024.0f / 1024.0f;
-	float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;
-
-	warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds));
-}
-
-/**
- * @return 0 if file exists
- */
-int file_exist(const char *filename)
-{
-	struct stat buffer;
-	return stat(filename, &buffer);
-}
-
-int file_copy(const char *file_old, const char *file_new)
-{
-	FILE *source, *target;
-	source = fopen(file_old, "r");
-	int ret = 0;
-
-	if (source == NULL) {
-		warnx("failed opening input file to copy");
-		return 1;
-	}
-
-	target = fopen(file_new, "w");
-
-	if (target == NULL) {
-		fclose(source);
-		warnx("failed to open output file to copy");
-		return 1;
-	}
-
-	char buf[128];
-	int nread;
-
-	while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) {
-		int ret = fwrite(buf, 1, nread, target);
-
-		if (ret <= 0) {
-			warnx("error writing file");
-			ret = 1;
-			break;
-		}
-	}
-
-	fsync(fileno(target));
-
-	fclose(source);
-	fclose(target);
-
-	return ret;
-}
-
-void handle_command(struct vehicle_command_s *cmd)
-{
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
-	/* request to set different system mode */
-	switch (cmd->command) {
-
-	case VEHICLE_CMD_PREFLIGHT_STORAGE:
-
-		if (((int)(cmd->param3)) == 1)	{
-
-			/* enable logging */
-			mavlink_log_info(mavlink_fd, "[log] file:");
-			mavlink_log_info(mavlink_fd, "logdir");
-			logging_enabled = true;
-		}
-		if (((int)(cmd->param3)) == 0)	{
-
-			/* disable logging */
-			mavlink_log_info(mavlink_fd, "[log] stopped.");
-			logging_enabled = false;
-		}
-		break;
-
-	default:
-		/* silently ignore */
-		break;
-	}
-
-}
diff --git a/src/modules/sdlog/sdlog_ringbuffer.h b/src/modules/sdlog/sdlog_ringbuffer.h
deleted file mode 100644
index b65916459..000000000
--- a/src/modules/sdlog/sdlog_ringbuffer.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   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
- * 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 sdlog_ringbuffer.h
- * microSD logging
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#ifndef SDLOG_RINGBUFFER_H_
-#define SDLOG_RINGBUFFER_H_
-
-#pragma pack(push, 1)
-struct sdlog_sysvector {
-	uint64_t timestamp;		/**<  time [us] */
-	float gyro[3];			/**< [rad/s] */
-	float accel[3];			/**< [m/s^2] */
-	float mag[3];			/**< [gauss] */
-	float baro;			/**< pressure [millibar] */
-	float baro_alt;			/**< altitude above MSL [meter] */
-	float baro_temp;		/**< [degree celcius] */
-	float control[4];		/**< roll, pitch, yaw [-1..1], thrust [0..1] */
-	float actuators[8];		/**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */
-	float vbat;			/**< battery voltage in [volt] */
-	float bat_current;		/**< battery discharge current */
-	float bat_discharged;		/**< discharged energy in mAh */
-	float adc[4];			/**< ADC ports [volt] */
-	float local_position[3];	/**< tangent plane mapping into x,y,z [m] */
-	int32_t gps_raw_position[3];	/**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */
-	float attitude[3];		/**< roll, pitch, yaw [rad] */
-	float rotMatrix[9];		/**< unitvectors */
-	float vicon[6];			/**< Vicon ground truth x, y, z and roll, pitch, yaw */
-	float control_effective[4]; 	/**< roll, pitch, yaw [-1..1], thrust [0..1] */
-	float flow[6];			/**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */
-	float diff_pressure;		/**< differential pressure */
-	float ind_airspeed;		/**< indicated airspeed */
-	float true_airspeed;		/**< true airspeed */
-};
-#pragma pack(pop)
-
-struct sdlog_logbuffer {
-	unsigned int start;
-	// unsigned int end;
-	unsigned int size;
-	int count;
-	struct sdlog_sysvector *elems;
-};
-
-void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size);
-
-int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb);
-
-int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb);
-
-void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem);
-
-int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem);
-
-#endif
diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c
index b3243f7b5..2da67d8a9 100644
--- a/src/modules/sdlog2/logbuffer.c
+++ b/src/modules/sdlog2/logbuffer.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   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
@@ -75,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 	// bytes available to write
 	int available = lb->read_ptr - lb->write_ptr - 1;
 
-	if (available < 0)
+	if (available < 0) {
 		available += lb->size;
+	}
 
 	if (size > available) {
 		// buffer overflow
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index f53129688..a28d43e72 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -41,3 +41,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
 
 SRCS = sdlog2.c \
        logbuffer.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index c4fafb5a6..0d36fa2c6 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.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
@@ -60,6 +58,8 @@
 #include <drivers/drv_hrt.h>
 #include <math.h>
 
+#include <drivers/drv_range_finder.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
@@ -72,8 +72,9 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -82,9 +83,16 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/servorail_status.h>
+#include <uORB/topics/wind_estimate.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
 #include <version/version.h>
 
 #include <mavlink/mavlink_log.h>
@@ -93,6 +101,36 @@
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
 
+/**
+ * Logging rate.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 sets the minimum rate,
+ * any other value is interpreted as rate in Hertz. This
+ * parameter is only read out before logging starts (which
+ * commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_RATE, -1);
+
+/**
+ * Enable extended logging mode.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 disables extended
+ * logging mode, a value of 1 enables it. This
+ * parameter is only read out before logging starts
+ * (which commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_EXT, -1);
+
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
 	} else { \
@@ -104,16 +142,20 @@
 	fds[fdsc_count].events = POLLIN; \
 	fdsc_count++;
 
+#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
+
 static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
 static bool logwriter_should_exit = false;	/**< Logwriter thread exit flag */
-static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
-static const int MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
+static const unsigned MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
+static const unsigned MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
 static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
 static const int MAX_WRITE_CHUNK = 512;
 static const int MIN_BYTES_TO_WRITE = 512;
 
+static bool _extended_logging = false;
+
 static const char *log_root = "/fs/microsd/log";
 static int mavlink_fd = -1;
 struct logbuffer_s lb;
@@ -135,12 +177,6 @@ static uint64_t gps_time = 0;
 
 /* current state of logging */
 static bool logging_enabled = false;
-/* enable logging on start (-e option) */
-static bool log_on_start = false;
-/* enable logging when armed (-a option) */
-static bool log_when_armed = false;
-/* delay = 1 / rate (rate defined by -r option) */
-static useconds_t sleep_delay = 0;
 /* use date/time for naming directories and files (-t option) */
 static bool log_name_timestamp = false;
 
@@ -160,6 +196,8 @@ static void *logwriter_thread(void *arg);
  */
 __EXPORT int sdlog2_main(int argc, char *argv[]);
 
+static bool copy_if_updated(orb_id_t topic, int handle, void *buffer);
+
 /**
  * Mainloop of sd log deamon.
  */
@@ -218,18 +256,22 @@ static int create_log_dir(void);
  */
 static int open_log_file(void);
 
+static int open_perf_file(const char* str);
+
 static void
 sdlog2_usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", 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");
+	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\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-x\tExtended logging");
 }
 
 /**
@@ -242,8 +284,9 @@ sdlog2_usage(const char *reason)
  */
 int sdlog2_main(int argc, char *argv[])
 {
-	if (argc < 2)
+	if (argc < 2) {
 		sdlog2_usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
@@ -255,11 +298,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);
 	}
 
@@ -347,8 +390,8 @@ int create_log_dir()
 int open_log_file()
 {
 	/* string to hold the path to the log */
-	char log_file_name[16] = "";
-	char log_file_path[48] = "";
+	char log_file_name[32] = "";
+	char log_file_path[64] = "";
 
 	if (log_name_timestamp && gps_time != 0) {
 		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
@@ -376,7 +419,7 @@ int open_log_file()
 
 		if (file_number > MAX_NO_LOGFILE) {
 			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-			warnx("all %d possible files exist already", MAX_NO_LOGFILE);
+			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
 			return -1;
 		}
 	}
@@ -385,7 +428,58 @@ int open_log_file()
 
 	if (fd < 0) {
 		warn("failed opening log: %s", log_file_name);
-		mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+
+	} else {
+		warnx("log file: %s", log_file_name);
+		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
+	}
+
+	return fd;
+}
+
+int open_perf_file(const char* str)
+{
+	/* string to hold the path to the log */
+	char log_file_name[32] = "";
+	char log_file_path[64] = "";
+
+	if (log_name_timestamp && gps_time != 0) {
+		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
+		time_t gps_time_sec = gps_time / 1000000;
+		struct tm t;
+		gmtime_r(&gps_time_sec, &t);
+		strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t);
+		snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
+
+	} else {
+		unsigned file_number = 1; // start with file log001
+
+		/* look for the next file that does not exist */
+		while (file_number <= MAX_NO_LOGFILE) {
+			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
+			snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number);
+			snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
+
+			if (!file_exist(log_file_path)) {
+				break;
+			}
+
+			file_number++;
+		}
+
+		if (file_number > MAX_NO_LOGFILE) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
+			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+			return -1;
+		}
+	}
+
+	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
+
+	if (fd < 0) {
+		warn("failed opening log: %s", log_file_name);
+		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
 
 	} else {
 		warnx("log file: %s", log_file_name);
@@ -402,22 +496,29 @@ static void *logwriter_thread(void *arg)
 
 	int log_fd = open_log_file();
 
-	if (log_fd < 0)
-		return;
+	if (log_fd < 0) {
+		return NULL;
+	}
 
 	struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
 
 	/* write log messages formats, version and parameters */
 	log_bytes_written += write_formats(log_fd);
+
 	log_bytes_written += write_version(log_fd);
+
 	log_bytes_written += write_parameters(log_fd);
+
 	fsync(log_fd);
 
 	int poll_count = 0;
 
 	void *read_ptr;
+
 	int n = 0;
+
 	bool should_wait = false;
+
 	bool is_part = false;
 
 	while (true) {
@@ -483,7 +584,7 @@ static void *logwriter_thread(void *arg)
 	fsync(log_fd);
 	close(log_fd);
 
-	return;
+	return NULL;
 }
 
 void sdlog2_start_log()
@@ -520,6 +621,12 @@ void sdlog2_start_log()
 		errx(1, "error creating logwriter thread");
 	}
 
+	/* write all performance counters */
+	int perf_fd = open_perf_file("preflight");
+	dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
+	perf_print_all(perf_fd);
+	close(perf_fd);
+
 	logging_enabled = true;
 }
 
@@ -547,6 +654,12 @@ void sdlog2_stop_log()
 	logwriter_pthread = 0;
 	pthread_attr_destroy(&logwriter_attr);
 
+	/* write all performance counters */
+	int perf_fd = open_perf_file("postflight");
+	dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
+	perf_print_all(perf_fd);
+	close(perf_fd);
+
 	sdlog2_status();
 }
 
@@ -563,7 +676,7 @@ int write_formats(int fd)
 	int written = 0;
 
 	/* fill message format packet for each format and write it */
-	for (int i = 0; i < log_formats_num; i++) {
+	for (unsigned i = 0; i < log_formats_num; i++) {
 		log_msg_format.body = log_formats[i];
 		written += write(fd, &log_msg_format, sizeof(log_msg_format));
 	}
@@ -628,6 +741,19 @@ int write_parameters(int fd)
 	return written;
 }
 
+bool copy_if_updated(orb_id_t topic, int handle, void *buffer)
+{
+	bool updated;
+
+	orb_check(handle, &updated);
+
+	if (updated) {
+		orb_copy(topic, handle, buffer);
+	}
+
+	return updated;
+}
+
 int sdlog2_thread_main(int argc, char *argv[])
 {
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@@ -636,12 +762,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		warnx("failed to open MAVLink log stream, start mavlink app first");
 	}
 
-	/* log buffer size */
+	/* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */
+	useconds_t sleep_delay = 20000;
 	int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
-
 	logging_enabled = false;
-	log_on_start = false;
-	log_when_armed = false;
+	/* enable logging on start (-e option) */
+	bool log_on_start = false;
+	/* enable logging when armed (-a option) */
+	bool log_when_armed = false;
 	log_name_timestamp = false;
 
 	flag_system_armed = false;
@@ -651,17 +779,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 	argv += 2;
 	int ch;
 
-	while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
+	/* don't exit from getopt loop to leave getopt global variables in consistent state,
+	 * set error flag instead */
+	bool err_flag = false;
+
+	while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) {
 		switch (ch) {
 		case 'r': {
 				unsigned long r = strtoul(optarg, NULL, 10);
 
 				if (r == 0) {
-					sleep_delay = 0;
-
-				} else {
-					sleep_delay = 1000000 / r;
+					r = 1;
 				}
+
+				sleep_delay = 1000000 / r;
 			}
 			break;
 
@@ -688,6 +819,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_name_timestamp = true;
 			break;
 
+		case 'x':
+			_extended_logging = true;
+			break;
+
 		case '?':
 			if (optopt == 'c') {
 				warnx("option -%c requires an argument", optopt);
@@ -698,20 +833,65 @@ int sdlog2_thread_main(int argc, char *argv[])
 			} else {
 				warnx("unknown option character `\\x%x'", optopt);
 			}
+			err_flag = true;
+			break;
 
 		default:
-			sdlog2_usage("unrecognized flag");
-			errx(1, "exiting");
+			warnx("unrecognized flag");
+			err_flag = true;
+			break;
 		}
 	}
 
+	if (err_flag) {
+		sdlog2_usage(NULL);
+	}
+
 	gps_time = 0;
 
+	/* interpret logging params */
+
+	param_t log_rate_ph = param_find("SDLOG_RATE");
+
+	if (log_rate_ph != PARAM_INVALID) {
+		int32_t param_log_rate;
+		param_get(log_rate_ph, &param_log_rate);
+
+		if (param_log_rate > 0) {
+
+			/* we can't do more than ~ 500 Hz, even with a massive buffer */
+			if (param_log_rate > 500) {
+				param_log_rate = 500;
+			}
+
+			sleep_delay = 1000000 / param_log_rate;
+		} else if (param_log_rate == 0) {
+			/* we need at minimum 10 Hz to be able to see anything */
+			sleep_delay = 1000000 / 10;
+		}
+	}
+
+	param_t log_ext_ph = param_find("SDLOG_EXT");
+
+	if (log_ext_ph != PARAM_INVALID) {
+
+		int32_t param_log_extended;
+		param_get(log_ext_ph, &param_log_extended);
+
+		if (param_log_extended > 0) {
+			_extended_logging = true;
+		} else if (param_log_extended == 0) {
+			_extended_logging = false;
+		}
+		/* any other value means to ignore the parameter, so no else case */
+
+	}
+
 	/* create log root dir */
 	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
 
 	if (mkdir_ret != 0 && errno != EEXIST) {
-		err("failed creating log root dir: %s", log_root);
+		err(1, "failed creating log root dir: %s", log_root);
 	}
 
 	/* copy conversion scripts */
@@ -734,8 +914,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	struct vehicle_status_s buf_status;
 
+	struct vehicle_gps_position_s buf_gps_pos;
+
 	memset(&buf_status, 0, sizeof(buf_status));
 
+	memset(&buf_gps_pos, 0, sizeof(buf_gps_pos));
+
 	/* warning! using union here to save memory, elements should be used separately! */
 	union {
 		struct vehicle_command_s cmd;
@@ -748,8 +932,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
-		struct vehicle_global_position_setpoint_s global_pos_sp;
-		struct vehicle_gps_position_s gps_pos;
+		struct position_setpoint_triplet_s triplet;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
 		struct rc_channels_s rc;
@@ -758,33 +941,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct esc_status_s esc;
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 		struct battery_status_s battery;
+		struct telemetry_status_s telemetry;
+		struct range_finder_report range_finder;
+		struct estimator_status_report estimator_status;
+		struct tecs_status_s tecs_status;
+		struct system_power_s system_power;
+		struct servorail_status_s servorail_status;
+		struct satellite_info_s sat_info;
+		struct wind_estimate_s wind_estimate;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
 
-	struct {
-		int cmd_sub;
-		int status_sub;
-		int sensor_sub;
-		int att_sub;
-		int att_sp_sub;
-		int rates_sp_sub;
-		int act_outputs_sub;
-		int act_controls_sub;
-		int local_pos_sub;
-		int local_pos_sp_sub;
-		int global_pos_sub;
-		int global_pos_sp_sub;
-		int gps_pos_sub;
-		int vicon_pos_sub;
-		int flow_sub;
-		int rc_sub;
-		int airspeed_sub;
-		int esc_sub;
-		int global_vel_sp_sub;
-		int battery_sub;
-	} subs;
-
 	/* log message buffer: header + body */
 #pragma pack(push, 1)
 	struct {
@@ -810,6 +978,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ESC_s log_ESC;
 			struct log_GVSP_s log_GVSP;
 			struct log_BATT_s log_BATT;
+			struct log_DIST_s log_DIST;
+			struct log_TEL_s log_TEL;
+			struct log_EST0_s log_EST0;
+			struct log_EST1_s log_EST1;
+			struct log_PWR_s log_PWR;
+			struct log_VICN_s log_VICN;
+			struct log_GS0A_s log_GS0A;
+			struct log_GS0B_s log_GS0B;
+			struct log_GS1A_s log_GS1A;
+			struct log_GS1B_s log_GS1B;
+			struct log_TECS_s log_TECS;
+			struct log_WIND_s log_WIND;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -817,148 +997,69 @@ int sdlog2_thread_main(int argc, char *argv[])
 #pragma pack(pop)
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
-	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 25;
-	/* Sanity check variable and index */
-	ssize_t fdsc_count = 0;
-	/* file descriptors to wait for */
-	struct pollfd fds[fdsc];
+	struct {
+		int cmd_sub;
+		int status_sub;
+		int sensor_sub;
+		int att_sub;
+		int att_sp_sub;
+		int rates_sp_sub;
+		int act_outputs_sub;
+		int act_controls_sub;
+		int local_pos_sub;
+		int local_pos_sp_sub;
+		int global_pos_sub;
+		int triplet_sub;
+		int gps_pos_sub;
+		int sat_info_sub;
+		int vicon_pos_sub;
+		int flow_sub;
+		int rc_sub;
+		int airspeed_sub;
+		int esc_sub;
+		int global_vel_sp_sub;
+		int battery_sub;
+		int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
+		int range_finder_sub;
+		int estimator_status_sub;
+		int tecs_status_sub;
+		int system_power_sub;
+		int servorail_status_sub;
+		int wind_sub;
+	} subs;
 
-	/* --- VEHICLE COMMAND --- */
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	fds[fdsc_count].fd = subs.cmd_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- VEHICLE STATUS --- */
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	fds[fdsc_count].fd = subs.status_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GPS POSITION --- */
 	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	fds[fdsc_count].fd = subs.gps_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- SENSORS COMBINED --- */
+	subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	fds[fdsc_count].fd = subs.sensor_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ATTITUDE --- */
 	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	fds[fdsc_count].fd = subs.att_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ATTITUDE SETPOINT --- */
 	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	fds[fdsc_count].fd = subs.att_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- RATES SETPOINT --- */
 	subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	fds[fdsc_count].fd = subs.rates_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ACTUATOR OUTPUTS --- */
 	subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
-	fds[fdsc_count].fd = subs.act_outputs_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ACTUATOR CONTROL --- */
 	subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	fds[fdsc_count].fd = subs.act_controls_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- LOCAL POSITION --- */
 	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	fds[fdsc_count].fd = subs.local_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- LOCAL POSITION SETPOINT --- */
 	subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	fds[fdsc_count].fd = subs.local_pos_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GLOBAL POSITION --- */
 	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	fds[fdsc_count].fd = subs.global_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GLOBAL POSITION SETPOINT--- */
-	subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	fds[fdsc_count].fd = subs.global_pos_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- VICON POSITION --- */
+	subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	fds[fdsc_count].fd = subs.vicon_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- OPTICAL FLOW --- */
 	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
-	fds[fdsc_count].fd = subs.flow_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- RC CHANNELS --- */
 	subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	fds[fdsc_count].fd = subs.rc_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- AIRSPEED --- */
 	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	fds[fdsc_count].fd = subs.airspeed_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ESCs --- */
 	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
-	fds[fdsc_count].fd = subs.esc_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GLOBAL VELOCITY SETPOINT --- */
 	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
-	fds[fdsc_count].fd = subs.global_vel_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- BATTERY --- */
 	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
-	fds[fdsc_count].fd = subs.battery_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* WARNING: If you get the error message below,
-	 * then the number of registered messages (fdsc)
-	 * differs from the number of messages in the above list.
-	 */
-	if (fdsc_count > fdsc) {
-		warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__);
-		fdsc_count = fdsc;
+	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
 	}
-
-	/*
-	 * set up poll to block for new data,
-	 * wait for a maximum of 1000 ms
-	 */
-	const int poll_timeout = 1000;
+	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
+	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
+	subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
+	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
+	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
+	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
+	/* we need to rate-limit wind, as we do not need the full update rate */
+	orb_set_interval(subs.wind_sub, 90);
 
 	thread_running = true;
 
@@ -967,18 +1068,21 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_cond_init(&logbuffer_cond, NULL);
 
 	/* track changes in sensor_combined topic */
-	uint16_t gyro_counter = 0;
-	uint16_t accelerometer_counter = 0;
-	uint16_t magnetometer_counter = 0;
-	uint16_t baro_counter = 0;
-	uint16_t differential_pressure_counter = 0;
+	hrt_abstime gyro_timestamp = 0;
+	hrt_abstime accelerometer_timestamp = 0;
+	hrt_abstime magnetometer_timestamp = 0;
+	hrt_abstime barometer_timestamp = 0;
+	hrt_abstime differential_pressure_timestamp = 0;
+
+	/* initialize calculated mean SNR */
+	float snr_mean = 0.0f;
 
 	/* enable logging on start if needed */
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
-			if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) {
-				gps_time = buf.gps_pos.time_gps_usec;
+			if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
+				gps_time = buf_gps_pos.time_gps_usec;
 			}
 		}
 
@@ -986,367 +1090,484 @@ int sdlog2_thread_main(int argc, char *argv[])
 	}
 
 	while (!main_thread_should_exit) {
-		/* decide use usleep() or blocking poll() */
-		bool use_sleep = sleep_delay > 0 && logging_enabled;
-
-		/* poll all topics if logging enabled or only management (first 2) if not */
-		int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
-
-		/* handle the poll result */
-		if (poll_ret < 0) {
-			warnx("ERROR: poll error, stop logging");
-			main_thread_should_exit = true;
-
-		} else if (poll_ret > 0) {
-
-			/* check all data subscriptions only if logging enabled,
-			 * logging_enabled can be changed while checking vehicle_command and vehicle_status */
-			bool check_data = logging_enabled;
-			int ifds = 0;
-			int handled_topics = 0;
-
-			/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
-				handle_command(&buf.cmd);
-				handled_topics++;
-			}
+		usleep(sleep_delay);
 
-			/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
+		/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
+		if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) {
+			handle_command(&buf.cmd);
+		}
 
-				if (log_when_armed) {
-					handle_status(&buf_status);
-				}
+		/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
+		bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
 
-				handled_topics++;
+		if (status_updated) {
+			if (log_when_armed) {
+				handle_status(&buf_status);
 			}
+		}
 
-			/* --- GPS POSITION - LOG MANAGEMENT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+		/* --- GPS POSITION - LOG MANAGEMENT --- */
+		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
 
-				if (log_name_timestamp) {
-					gps_time = buf.gps_pos.time_gps_usec;
-				}
+		if (gps_pos_updated && log_name_timestamp) {
+			gps_time = buf_gps_pos.time_gps_usec;
+		}
 
-				handled_topics++;
-			}
+		if (!logging_enabled) {
+			continue;
+		}
 
-			if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
-				continue;
-			}
+		pthread_mutex_lock(&logbuffer_mutex);
 
-			ifds = 1;	// begin from fds[1] again
-
-			pthread_mutex_lock(&logbuffer_mutex);
-
-			/* write time stamp message */
-			log_msg.msg_type = LOG_TIME_MSG;
-			log_msg.body.log_TIME.t = hrt_absolute_time();
-			LOGBUFFER_WRITE_AND_COUNT(TIME);
-
-			/* --- VEHICLE STATUS --- */
-			if (fds[ifds++].revents & POLLIN) {
-				// Don't orb_copy, it's already done few lines above
-				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.navigation_state = (uint8_t) buf_status.navigation_state;
-				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
-				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;
-				LOGBUFFER_WRITE_AND_COUNT(STAT);
-			}
+		/* write time stamp message */
+		log_msg.msg_type = LOG_TIME_MSG;
+		log_msg.body.log_TIME.t = hrt_absolute_time();
+		LOGBUFFER_WRITE_AND_COUNT(TIME);
+
+		/* --- VEHICLE STATUS --- */
+		if (status_updated) {
+			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;
+			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;
+			LOGBUFFER_WRITE_AND_COUNT(STAT);
+		}
 
-			/* --- GPS POSITION --- */
-			if (fds[ifds++].revents & POLLIN) {
-				// Don't orb_copy, it's already done few lines above
-				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.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;
-				log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s;
-				log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s;
-				log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s;
-				log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad;
-				LOGBUFFER_WRITE_AND_COUNT(GPS);
-			}
+		/* --- GPS POSITION - UNIT #1 --- */
+		if (gps_pos_updated) {
+
+			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;
+			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;
+			log_msg.body.log_GPS.vel_n = buf_gps_pos.vel_n_m_s;
+			log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
+			log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
+			log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
+			log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used;
+			log_msg.body.log_GPS.snr_mean = snr_mean;
+			log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
+			log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
+			LOGBUFFER_WRITE_AND_COUNT(GPS);
+		}
 
-			/* --- SENSOR COMBINED --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
-				bool write_IMU = false;
-				bool write_SENS = false;
+		/* --- SATELLITE INFO - UNIT #1 --- */
+		if (_extended_logging) {
 
-				if (buf.sensor.gyro_counter != gyro_counter) {
-					gyro_counter = buf.sensor.gyro_counter;
-					write_IMU = true;
-				}
+			if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
 
-				if (buf.sensor.accelerometer_counter != accelerometer_counter) {
-					accelerometer_counter = buf.sensor.accelerometer_counter;
-					write_IMU = true;
-				}
+				/* log the SNR of each satellite for a detailed view of signal quality */
+				unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
+				unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
 
-				if (buf.sensor.magnetometer_counter != magnetometer_counter) {
-					magnetometer_counter = buf.sensor.magnetometer_counter;
-					write_IMU = true;
-				}
+				log_msg.msg_type = LOG_GS0A_MSG;
+				memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
+				snr_mean = 0.0f;
 
-				if (buf.sensor.baro_counter != baro_counter) {
-					baro_counter = buf.sensor.baro_counter;
-					write_SENS = true;
-				}
+				/* fill set A and calculate mean SNR */
+				for (unsigned i = 0; i < sat_info_count; i++) {
 
-				if (buf.sensor.differential_pressure_counter != differential_pressure_counter) {
-					differential_pressure_counter = buf.sensor.differential_pressure_counter;
-					write_SENS = true;
-				}
+					snr_mean += buf.sat_info.snr[i];
 
-				if (write_IMU) {
-					log_msg.msg_type = LOG_IMU_MSG;
-					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
-					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
-					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
-					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
-					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
-					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
-					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
-					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
-					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
-					LOGBUFFER_WRITE_AND_COUNT(IMU);
+					int satindex = buf.sat_info.svid[i] - 1;
+
+					/* handles index exceeding and wraps to to arithmetic errors */
+					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
+						/* map satellites by their ID so that logs from two receivers can be compared */
+						log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i];
+					}
 				}
+				LOGBUFFER_WRITE_AND_COUNT(GS0A);
+				snr_mean /= sat_info_count;
+
+				log_msg.msg_type = LOG_GS0B_MSG;
+				memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
+
+				/* fill set B */
+				for (unsigned i = 0; i < sat_info_count; i++) {
+
+					/* get second bank of satellites, thus deduct bank size from index */
+					int satindex = buf.sat_info.svid[i] - 1 - log_max_snr;
 
-				if (write_SENS) {
-					log_msg.msg_type = LOG_SENS_MSG;
-					log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
-					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
-					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
-					log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
-					LOGBUFFER_WRITE_AND_COUNT(SENS);
+					/* handles index exceeding and wraps to to arithmetic errors */
+					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
+						/* map satellites by their ID so that logs from two receivers can be compared */
+						log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i];
+					}
 				}
+				LOGBUFFER_WRITE_AND_COUNT(GS0B);
 			}
+		}
 
-			/* --- ATTITUDE --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
-				log_msg.msg_type = LOG_ATT_MSG;
-				log_msg.body.log_ATT.roll = buf.att.roll;
-				log_msg.body.log_ATT.pitch = buf.att.pitch;
-				log_msg.body.log_ATT.yaw = buf.att.yaw;
-				log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
-				log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
-				log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
-				LOGBUFFER_WRITE_AND_COUNT(ATT);
-			}
+		/* --- SENSOR COMBINED --- */
+		if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
+			bool write_IMU = false;
+			bool write_SENS = false;
 
-			/* --- ATTITUDE SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp);
-				log_msg.msg_type = LOG_ATSP_MSG;
-				log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
-				log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
-				log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body;
-				log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust;
-				LOGBUFFER_WRITE_AND_COUNT(ATSP);
+			if (buf.sensor.timestamp != gyro_timestamp) {
+				gyro_timestamp = buf.sensor.timestamp;
+				write_IMU = true;
 			}
 
-			/* --- RATES SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp);
-				log_msg.msg_type = LOG_ARSP_MSG;
-				log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
-				log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
-				log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw;
-				LOGBUFFER_WRITE_AND_COUNT(ARSP);
+			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
+				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
+				write_IMU = true;
 			}
 
-			/* --- ACTUATOR OUTPUTS --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs);
-				log_msg.msg_type = LOG_OUT0_MSG;
-				memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
-				LOGBUFFER_WRITE_AND_COUNT(OUT0);
+			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
+				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
+				write_IMU = true;
 			}
 
-			/* --- ACTUATOR CONTROL --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls);
-				log_msg.msg_type = LOG_ATTC_MSG;
-				log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
-				log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
-				log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
-				log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
-				LOGBUFFER_WRITE_AND_COUNT(ATTC);
+			if (buf.sensor.baro_timestamp != barometer_timestamp) {
+				barometer_timestamp = buf.sensor.baro_timestamp;
+				write_SENS = true;
 			}
 
-			/* --- LOCAL POSITION --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
-				log_msg.msg_type = LOG_LPOS_MSG;
-				log_msg.body.log_LPOS.x = buf.local_pos.x;
-				log_msg.body.log_LPOS.y = buf.local_pos.y;
-				log_msg.body.log_LPOS.z = buf.local_pos.z;
-				log_msg.body.log_LPOS.vx = buf.local_pos.vx;
-				log_msg.body.log_LPOS.vy = buf.local_pos.vy;
-				log_msg.body.log_LPOS.vz = buf.local_pos.vz;
-				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
-				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
-				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
-				log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
-				log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
-				log_msg.body.log_LPOS.landed = buf.local_pos.landed;
-				LOGBUFFER_WRITE_AND_COUNT(LPOS);
+			if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
+				differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
+				write_SENS = true;
 			}
 
-			/* --- LOCAL POSITION SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp);
-				log_msg.msg_type = LOG_LPSP_MSG;
-				log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
-				log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
-				log_msg.body.log_LPSP.z = buf.local_pos_sp.z;
-				log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw;
-				LOGBUFFER_WRITE_AND_COUNT(LPSP);
+			if (write_IMU) {
+				log_msg.msg_type = LOG_IMU_MSG;
+				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
+				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
+				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
+				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
+				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
+				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
+				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
+				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
+				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
+				LOGBUFFER_WRITE_AND_COUNT(IMU);
 			}
 
-			/* --- GLOBAL POSITION --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
-				log_msg.msg_type = LOG_GPOS_MSG;
-				log_msg.body.log_GPOS.lat = buf.global_pos.lat;
-				log_msg.body.log_GPOS.lon = buf.global_pos.lon;
-				log_msg.body.log_GPOS.alt = buf.global_pos.alt;
-				log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
-				log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
-				log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
-				LOGBUFFER_WRITE_AND_COUNT(GPOS);
+			if (write_SENS) {
+				log_msg.msg_type = LOG_SENS_MSG;
+				log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
+				log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
+				log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
+				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
+				log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
+				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
 
-			/* --- GLOBAL POSITION SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
-				log_msg.msg_type = LOG_GPSP_MSG;
-				log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
-				log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
-				log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
-				log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
-				log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
-				log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
-				log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
-				log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
-				log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
-				log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
-				log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
-				log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
-				LOGBUFFER_WRITE_AND_COUNT(GPSP);
-			}
+		}
 
-			/* --- VICON POSITION --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
-				// TODO not implemented yet
-			}
+		/* --- ATTITUDE --- */
+		if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
+			log_msg.msg_type = LOG_ATT_MSG;
+			log_msg.body.log_ATT.roll = buf.att.roll;
+			log_msg.body.log_ATT.pitch = buf.att.pitch;
+			log_msg.body.log_ATT.yaw = buf.att.yaw;
+			log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
+			log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
+			log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
+			log_msg.body.log_ATT.gx = buf.att.g_comp[0];
+			log_msg.body.log_ATT.gy = buf.att.g_comp[1];
+			log_msg.body.log_ATT.gz = buf.att.g_comp[2];
+			LOGBUFFER_WRITE_AND_COUNT(ATT);
+		}
 
-			/* --- FLOW --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
-				log_msg.msg_type = LOG_FLOW_MSG;
-				log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
-				log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
-				log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
-				log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
-				log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
-				log_msg.body.log_FLOW.quality = buf.flow.quality;
-				log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
-				LOGBUFFER_WRITE_AND_COUNT(FLOW);
-			}
+		/* --- ATTITUDE SETPOINT --- */
+		if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) {
+			log_msg.msg_type = LOG_ATSP_MSG;
+			log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
+			log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
+			log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body;
+			log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust;
+			LOGBUFFER_WRITE_AND_COUNT(ATSP);
+		}
 
-			/* --- RC CHANNELS --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc);
-				log_msg.msg_type = LOG_RC_MSG;
-				/* Copy only the first 8 channels of 14 */
-				memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
-				log_msg.body.log_RC.channel_count = buf.rc.chan_count;
-				LOGBUFFER_WRITE_AND_COUNT(RC);
-			}
+		/* --- RATES SETPOINT --- */
+		if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) {
+			log_msg.msg_type = LOG_ARSP_MSG;
+			log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
+			log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
+			log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw;
+			LOGBUFFER_WRITE_AND_COUNT(ARSP);
+		}
 
-			/* --- AIRSPEED --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
-				log_msg.msg_type = LOG_AIRS_MSG;
-				log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
-				log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
-				LOGBUFFER_WRITE_AND_COUNT(AIRS);
-			}
+		/* --- ACTUATOR OUTPUTS --- */
+		if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) {
+			log_msg.msg_type = LOG_OUT0_MSG;
+			memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
+			LOGBUFFER_WRITE_AND_COUNT(OUT0);
+		}
 
-			/* --- ESCs --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
-
-				for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
-					log_msg.msg_type = LOG_ESC_MSG;
-					log_msg.body.log_ESC.counter = buf.esc.counter;
-					log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
-					log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
-					log_msg.body.log_ESC.esc_num = i;
-					log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
-					log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
-					log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
-					log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
-					log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
-					log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
-					log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
-					log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
-					LOGBUFFER_WRITE_AND_COUNT(ESC);
-				}
-			}
+		/* --- ACTUATOR CONTROL --- */
+		if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) {
+			log_msg.msg_type = LOG_ATTC_MSG;
+			log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
+			log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
+			log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
+			log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
+			LOGBUFFER_WRITE_AND_COUNT(ATTC);
+		}
 
-			/* --- GLOBAL VELOCITY SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
-				log_msg.msg_type = LOG_GVSP_MSG;
-				log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
-				log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
-				log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
-				LOGBUFFER_WRITE_AND_COUNT(GVSP);
-			}
+		/* --- LOCAL POSITION --- */
+		if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
+			log_msg.msg_type = LOG_LPOS_MSG;
+			log_msg.body.log_LPOS.x = buf.local_pos.x;
+			log_msg.body.log_LPOS.y = buf.local_pos.y;
+			log_msg.body.log_LPOS.z = buf.local_pos.z;
+			log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
+			log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
+			log_msg.body.log_LPOS.vx = buf.local_pos.vx;
+			log_msg.body.log_LPOS.vy = buf.local_pos.vy;
+			log_msg.body.log_LPOS.vz = buf.local_pos.vz;
+			log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7;
+			log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7;
+			log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
+			log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) |
+											  (buf.local_pos.z_valid ? 2 : 0) |
+											  (buf.local_pos.v_xy_valid ? 4 : 0) |
+											  (buf.local_pos.v_z_valid ? 8 : 0) |
+											  (buf.local_pos.xy_global ? 16 : 0) |
+											  (buf.local_pos.z_global ? 32 : 0);
+			log_msg.body.log_LPOS.landed = buf.local_pos.landed;
+			log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
+			log_msg.body.log_LPOS.eph = buf.local_pos.eph;
+			log_msg.body.log_LPOS.epv = buf.local_pos.epv;
+			LOGBUFFER_WRITE_AND_COUNT(LPOS);
+		}
+
+		/* --- LOCAL POSITION SETPOINT --- */
+		if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) {
+			log_msg.msg_type = LOG_LPSP_MSG;
+			log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
+			log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
+			log_msg.body.log_LPSP.z = buf.local_pos_sp.z;
+			log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw;
+			LOGBUFFER_WRITE_AND_COUNT(LPSP);
+		}
+
+		/* --- GLOBAL POSITION --- */
+		if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) {
+			log_msg.msg_type = LOG_GPOS_MSG;
+			log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
+			log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
+			log_msg.body.log_GPOS.alt = buf.global_pos.alt;
+			log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
+			log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
+			log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
+			log_msg.body.log_GPOS.eph = buf.global_pos.eph;
+			log_msg.body.log_GPOS.epv = buf.global_pos.epv;
+			LOGBUFFER_WRITE_AND_COUNT(GPOS);
+		}
+
+		/* --- 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 = 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;
+			log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
+			log_msg.body.log_GPSP.type = buf.triplet.current.type;
+			log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+			log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+			log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
+			LOGBUFFER_WRITE_AND_COUNT(GPSP);
+		}
+
+		/* --- VICON POSITION --- */
+		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
+			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 --- */
+		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
+			log_msg.msg_type = LOG_FLOW_MSG;
+			log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
+			log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
+			log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
+			log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
+			log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
+			log_msg.body.log_FLOW.quality = buf.flow.quality;
+			log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
+			LOGBUFFER_WRITE_AND_COUNT(FLOW);
+		}
+
+		/* --- RC CHANNELS --- */
+		if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
+			log_msg.msg_type = LOG_RC_MSG;
+			/* Copy only the first 8 channels of 14 */
+			memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel));
+			log_msg.body.log_RC.channel_count = buf.rc.channel_count;
+			log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
+			LOGBUFFER_WRITE_AND_COUNT(RC);
+		}
+
+		/* --- AIRSPEED --- */
+		if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) {
+			log_msg.msg_type = LOG_AIRS_MSG;
+			log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
+			log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
+			log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
+			LOGBUFFER_WRITE_AND_COUNT(AIRS);
+		}
 
-			/* --- BATTERY --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
-				log_msg.msg_type = LOG_BATT_MSG;
-				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
-				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
-				log_msg.body.log_BATT.current = buf.battery.current_a;
-				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
-				LOGBUFFER_WRITE_AND_COUNT(BATT);
+		/* --- ESCs --- */
+		if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) {
+			for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
+				log_msg.msg_type = LOG_ESC_MSG;
+				log_msg.body.log_ESC.counter = buf.esc.counter;
+				log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
+				log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
+				log_msg.body.log_ESC.esc_num = i;
+				log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
+				log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
+				log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
+				log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
+				log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
+				log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
+				log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
+				log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
+				LOGBUFFER_WRITE_AND_COUNT(ESC);
 			}
+		}
 
-			/* signal the other thread new data, but not yet unlock */
-			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
-				/* only request write if several packets can be written at once */
-				pthread_cond_signal(&logbuffer_cond);
+		/* --- GLOBAL VELOCITY SETPOINT --- */
+		if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) {
+			log_msg.msg_type = LOG_GVSP_MSG;
+			log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
+			log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
+			log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
+			LOGBUFFER_WRITE_AND_COUNT(GVSP);
+		}
+
+		/* --- BATTERY --- */
+		if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) {
+			log_msg.msg_type = LOG_BATT_MSG;
+			log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
+			log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
+			log_msg.body.log_BATT.current = buf.battery.current_a;
+			log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
+			LOGBUFFER_WRITE_AND_COUNT(BATT);
+		}
+
+		/* --- SYSTEM POWER RAILS --- */
+		if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
+			log_msg.msg_type = LOG_PWR_MSG;
+			log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
+			log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
+			log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
+			log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
+			log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
+			log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
+
+			/* copy servo rail status topic here too */
+			orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
+			log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
+			log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
+
+			LOGBUFFER_WRITE_AND_COUNT(PWR);
+		}
+
+		/* --- TELEMETRY --- */
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
+				log_msg.msg_type = LOG_TEL0_MSG + i;
+				log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
+				log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
+				log_msg.body.log_TEL.noise = buf.telemetry.noise;
+				log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise;
+				log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors;
+				log_msg.body.log_TEL.fixed = buf.telemetry.fixed;
+				log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf;
+				log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time;
+				LOGBUFFER_WRITE_AND_COUNT(TEL);
 			}
+		}
+
+		/* --- BOTTOM DISTANCE --- */
+		if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
+			log_msg.msg_type = LOG_DIST_MSG;
+			log_msg.body.log_DIST.bottom = buf.range_finder.distance;
+			log_msg.body.log_DIST.bottom_rate = 0.0f;
+			log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
+			LOGBUFFER_WRITE_AND_COUNT(DIST);
+		}
+
+		/* --- ESTIMATOR STATUS --- */
+		if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
+			log_msg.msg_type = LOG_EST0_MSG;
+			unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s);
+			memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s));
+			memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0);
+			log_msg.body.log_EST0.n_states = buf.estimator_status.n_states;
+			log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags;
+			log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags;
+			log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags;
+			LOGBUFFER_WRITE_AND_COUNT(EST0);
+
+			log_msg.msg_type = LOG_EST1_MSG;
+			unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s);
+			memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s));
+			memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1);
+			LOGBUFFER_WRITE_AND_COUNT(EST1);
+		}
+
+		/* --- TECS STATUS --- */
+		if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
+			log_msg.msg_type = LOG_TECS_MSG;
+			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
+			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
+			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
+			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
+			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
+			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
+			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
+			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
+			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
+			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
+			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
+			log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
+			log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
+			log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
+			log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
+			LOGBUFFER_WRITE_AND_COUNT(TECS);
+		}
 
-			/* unlock, now the writer thread may run */
-			pthread_mutex_unlock(&logbuffer_mutex);
+		/* --- WIND ESTIMATE --- */
+		if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
+			log_msg.msg_type = LOG_WIND_MSG;
+			log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
+			log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
+			log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north;
+			log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east;
+			LOGBUFFER_WRITE_AND_COUNT(WIND);
 		}
 
-		if (use_sleep) {
-			usleep(sleep_delay);
+		/* signal the other thread new data, but not yet unlock */
+		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
+			/* only request write if several packets can be written at once */
+			pthread_cond_signal(&logbuffer_cond);
 		}
+
+		/* unlock, now the writer thread may run */
+		pthread_mutex_unlock(&logbuffer_mutex);
 	}
 
-	if (logging_enabled)
+	if (logging_enabled) {
 		sdlog2_stop_log();
+	}
 
 	pthread_mutex_destroy(&logbuffer_mutex);
 	pthread_cond_destroy(&logbuffer_cond);
@@ -1367,6 +1588,7 @@ void sdlog2_status()
 	float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
 
 	warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
+	warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF");
 	mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
 }
 
@@ -1421,8 +1643,6 @@ int file_copy(const char *file_old, const char *file_new)
 
 void handle_command(struct vehicle_command_s *cmd)
 {
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 	int param;
 
 	/* request to set different system mode */
diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h
index dc5e6c8bd..aff0e3f48 100644
--- a/src/modules/sdlog2/sdlog2_format.h
+++ b/src/modules/sdlog2/sdlog2_format.h
@@ -91,6 +91,14 @@ struct log_format_s {
 						    .labels = _labels \
 	}
 
+#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \
+		.type = LOG_##_name##_MSG, \
+			.length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \
+				  .name = #_name, \
+					  .format = _format, \
+						    .labels = _labels \
+	}
+
 #define LOG_FORMAT_MSG	  0x80
 
 #define LOG_PACKET_SIZE(_name)	LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s)
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a784a1f30..87f7f718f 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -57,6 +57,9 @@ struct log_ATT_s {
 	float roll_rate;
 	float pitch_rate;
 	float yaw_rate;
+	float gx;
+	float gy;
+	float gz;
 };
 
 /* --- ATSP - ATTITUDE SET POINT --- */
@@ -89,6 +92,7 @@ struct log_SENS_s {
 	float baro_alt;
 	float baro_temp;
 	float diff_pres;
+	float diff_pres_filtered;
 };
 
 /* --- LPOS - LOCAL POSITION --- */
@@ -97,15 +101,19 @@ struct log_LPOS_s {
 	float x;
 	float y;
 	float z;
+	float ground_dist;
+	float ground_dist_rate;
 	float vx;
 	float vy;
 	float vz;
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
-	uint8_t xy_flags;
-	uint8_t z_flags;
+	uint8_t pos_flags;
 	uint8_t landed;
+	uint8_t ground_dist_flags;
+	float eph;
+	float epv;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -131,6 +139,10 @@ struct log_GPS_s {
 	float vel_e;
 	float vel_d;
 	float cog;
+	uint8_t sats;
+	uint16_t snr_mean;
+	uint16_t noise_per_ms;
+	uint16_t jamming_indicator;
 };
 
 /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
@@ -146,8 +158,8 @@ struct log_ATTC_s {
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
 	uint8_t main_state;
-	uint8_t navigation_state;
 	uint8_t arming_state;
+	uint8_t failsafe_state;
 	float battery_remaining;
 	uint8_t battery_warning;
 	uint8_t landed;
@@ -158,6 +170,7 @@ struct log_STAT_s {
 struct log_RC_s {
 	float channel[8];
 	uint8_t channel_count;
+	uint8_t signal_lost;
 };
 
 /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
@@ -171,6 +184,7 @@ struct log_OUT0_s {
 struct log_AIRS_s {
 	float indicated_airspeed;
 	float true_airspeed;
+	float air_temperature_celsius;
 };
 
 /* --- ARSP - ATTITUDE RATE SET POINT --- */
@@ -202,23 +216,22 @@ struct log_GPOS_s {
 	float vel_n;
 	float vel_e;
 	float vel_d;
+	float eph;
+	float epv;
 };
 
 /* --- GPSP - GLOBAL POSITION SETPOINT --- */
 #define LOG_GPSP_MSG 17
 struct log_GPSP_s {
-	uint8_t altitude_is_relative;
+	uint8_t nav_state;
 	int32_t lat;
 	int32_t lon;
-	float altitude;
+	float alt;
 	float yaw;
+	uint8_t type;
 	float loiter_radius;
 	int8_t loiter_direction;
-	uint8_t nav_cmd;
-	float param1;
-	float param2;
-	float param3;
-	float param4;
+	float pitch_min;
 };
 
 /* --- ESC - ESC STATE --- */
@@ -255,6 +268,131 @@ struct log_BATT_s {
 	float discharged;
 };
 
+/* --- DIST - DISTANCE TO SURFACE --- */
+#define LOG_DIST_MSG 21
+struct log_DIST_s {
+	float bottom;
+	float bottom_rate;
+	uint8_t flags;
+};
+
+// ID 22 available
+// ID 23 available
+
+/* --- PWR - ONBOARD POWER SYSTEM --- */
+#define LOG_PWR_MSG 24
+struct log_PWR_s {
+	float peripherals_5v;
+	float servo_rail_5v;
+	float servo_rssi;
+	uint8_t usb_ok;
+	uint8_t brick_ok;
+	uint8_t servo_ok;
+	uint8_t low_power_rail_overcurrent;
+	uint8_t high_power_rail_overcurrent;
+};
+
+/* --- VICN - VICON POSITION --- */
+#define LOG_VICN_MSG 25
+struct log_VICN_s {
+	float x;
+	float y;
+	float z;
+	float roll;
+	float pitch;
+	float yaw;
+};
+
+/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
+#define LOG_GS0A_MSG 26
+struct log_GS0A_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
+/* --- GS0B - GPS SNR #0, SAT GROUP B --- */
+#define LOG_GS0B_MSG 27
+struct log_GS0B_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
+/* --- GS1A - GPS SNR #1, SAT GROUP A --- */
+#define LOG_GS1A_MSG 28
+struct log_GS1A_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
+/* --- GS1B - GPS SNR #1, SAT GROUP B --- */
+#define LOG_GS1B_MSG 29
+struct log_GS1B_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
+/* --- TECS - TECS STATUS --- */
+#define LOG_TECS_MSG 30
+struct log_TECS_s {
+	float altitudeSp;
+	float altitude;
+	float flightPathAngleSp;
+	float flightPathAngle;
+	float flightPathAngleFiltered;
+	float airspeedSp;
+	float airspeed;
+	float airspeedFiltered;
+	float airspeedDerivativeSp;
+	float airspeedDerivative;
+
+	float totalEnergyRateSp;
+	float totalEnergyRate;
+	float energyDistributionRateSp;
+	float energyDistributionRate;
+
+	uint8_t mode;
+};
+
+/* --- WIND - WIND ESTIMATE --- */
+#define LOG_WIND_MSG 31
+struct log_WIND_s {
+	float x;
+	float y;
+	float cov_x;
+	float cov_y;
+};
+
+/* --- EST0 - ESTIMATOR STATUS --- */
+#define LOG_EST0_MSG 32
+struct log_EST0_s {
+	float s[12];
+	uint8_t n_states;
+	uint8_t nan_flags;
+	uint8_t health_flags;
+	uint8_t timeout_flags;
+};
+
+/* --- EST1 - ESTIMATOR STATUS --- */
+#define LOG_EST1_MSG 33
+struct log_EST1_s {
+	float s[16];
+};
+
+/* --- TEL0..3 - TELEMETRY STATUS --- */
+#define LOG_TEL0_MSG 34
+#define LOG_TEL1_MSG 35
+#define LOG_TEL2_MSG 36
+#define LOG_TEL3_MSG 37
+struct log_TEL_s {
+	uint8_t rssi;
+	uint8_t remote_rssi;
+	uint8_t noise;
+	uint8_t remote_noise;
+	uint16_t rxerrors;
+	uint16_t fixed;
+	uint8_t txbuf;
+	uint64_t heartbeat_time;
+};
+
+
+/********** SYSTEM MESSAGES, ID > 0x80 **********/
+
 /* --- TIME - TIME STAMP --- */
 #define LOG_TIME_MSG 129
 struct log_TIME_s {
@@ -280,32 +418,48 @@ struct log_PARM_s {
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
-	LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
-	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
-	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
-	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
-	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
-	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
-	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
-	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
-	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
-	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
-	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
-	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
-	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
-	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
-	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
-	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
-	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
+	LOG_FORMAT(ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
+	LOG_FORMAT(IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
+	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
+	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
+	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
+	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
+	LOG_FORMAT(STAT, "BBBfBB",		"MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
+	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
+	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
+	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
+	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
+	LOG_FORMAT(GPOS, "LLffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
+	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
+	LOG_FORMAT(ESC, "HBBBHHHHHHfH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
+	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
+	LOG_FORMAT(DIST, "ffB",			"Bottom,BottomRate,Flags"),
+	LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
+	LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
+	LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
+	LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
+	LOG_FORMAT(EST0, "ffffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
+	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
+	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
+	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
+	LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
+
 	/* system-level messages, ID >= 0x80 */
-	// FMT: don't write format of format message, it's useless
+	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),
 	LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
-	LOG_FORMAT(PARM, "Nf", "Name,Value"),
+	LOG_FORMAT(PARM, "Nf", "Name,Value")
 };
 
-static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
+static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
 
 #endif /* SDLOG2_MESSAGES_H_ */
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp
index 96a443c6e..91230a37c 100644
--- a/src/modules/segway/BlockSegwayController.cpp
+++ b/src/modules/segway/BlockSegwayController.cpp
@@ -35,8 +35,8 @@ void BlockSegwayController::update() {
 
 	// handle autopilot modes
 	if (_status.main_state == MAIN_STATE_AUTO ||
-	    _status.main_state == MAIN_STATE_SEATBELT ||
-	    _status.main_state == MAIN_STATE_EASY) {
+	    _status.main_state == MAIN_STATE_ALTCTL ||
+	    _status.main_state == MAIN_STATE_POSCTL) {
 		_actuators.control[0] = spdCmd;
 		_actuators.control[1] = spdCmd;
 
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index aa538fd6b..5b1bc5e86 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -40,3 +40,5 @@ MODULE_PRIORITY	= "SCHED_PRIORITY_MAX-5"
 
 SRCS		= sensors.cpp \
 		  sensor_params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 30659fd3a..38b190761 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -42,13 +42,10 @@
  */
 
 #include <nuttx/config.h>
-
 #include <systemlib/param/param.h>
 
 /**
- * Gyro X offset
- *
- * This is an X-axis offset for the gyro. Adjust it according to the calibration data.
+ * Gyro X-axis offset
  *
  * @min -10.0
  * @max 10.0
@@ -57,7 +54,7 @@
 PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
 
 /**
- * Gyro Y offset
+ * Gyro Y-axis offset
  *
  * @min -10.0
  * @max 10.0
@@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
 
 /**
- * Gyro Z offset
+ * Gyro Z-axis offset
  *
  * @min -5.0
  * @max 5.0
@@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
 /**
- * Gyro X scaling
- *
- * X-axis scaling.
+ * Gyro X-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
 
 /**
- * Gyro Y scaling
- *
- * Y-axis scaling.
+ * Gyro Y-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
 
 /**
- * Gyro Z scaling
- *
- * Z-axis scaling.
+ * Gyro Z-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
 
+
 /**
- * Magnetometer X offset
- *
- * This is an X-axis offset for the magnetometer.
+ * Magnetometer X-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
 
 /**
- * Magnetometer Y offset
- *
- * This is an Y-axis offset for the magnetometer.
+ * Magnetometer Y-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
 
 /**
- * Magnetometer Z offset
- *
- * This is an Z-axis offset for the magnetometer.
+ * Magnetometer Z-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -140,24 +126,173 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
 
+/**
+ * Magnetometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
+
+/**
+ * Magnetometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
+
+/**
+ * Magnetometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
 
+
+/**
+ * Accelerometer X-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
+
+/**
+ * Accelerometer Y-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
+
+/**
+ * Accelerometer Z-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
 
+/**
+ * Accelerometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
+
+/**
+ * Accelerometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
+
+/**
+ * Accelerometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
+
+/**
+ * Differential pressure sensor offset
+ *
+ * The offset (zero-reading) in Pascal
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
-PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 
+/**
+ * Differential pressure sensor analog scaling
+ *
+ * Pick the appropriate scaling from the datasheet.
+ * this number defines the (linear) conversion from voltage
+ * to Pascal (pa). For the MPXV7002DP this is 1000.
+ *
+ * NOTE: If the sensor always registers zero, try switching
+ * the static and dynamic tubes.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
+
+
+/**
+ * Board rotation
+ *
+ * This parameter defines the rotation of the FMU board relative to the platform.
+ * Possible values are:
+ *    0 = No rotation
+ *    1 = Yaw 45°
+ *    2 = Yaw 90°
+ *    3 = Yaw 135°
+ *    4 = Yaw 180°
+ *    5 = Yaw 225°
+ *    6 = Yaw 270°
+ *    7 = Yaw 315°
+ *    8 = Roll 180°
+ *    9 = Roll 180°, Yaw 45°
+ *   10 = Roll 180°, Yaw 90°
+ *   11 = Roll 180°, Yaw 135°
+ *   12 = Pitch 180°
+ *   13 = Roll 180°, Yaw 225°
+ *   14 = Roll 180°, Yaw 270°
+ *   15 = Roll 180°, Yaw 315°
+ *   16 = Roll 90°
+ *   17 = Roll 90°, Yaw 45°
+ *   18 = Roll 90°, Yaw 90°
+ *   19 = Roll 90°, Yaw 135°
+ *   20 = Roll 270°
+ *   21 = Roll 270°, Yaw 45°
+ *   22 = Roll 270°, Yaw 90°
+ *   23 = Roll 270°, Yaw 135°
+ *   24 = Pitch 90°
+ *   25 = Pitch 270°
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+
+/**
+ * Board rotation Y (Pitch) offset
+ *
+ * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
+
+/**
+ * Board rotation X (Roll) offset
+ *
+ * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
+
+/**
+ * Board rotation Z (YAW) offset
+ *
+ * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
+
+/**
+ * External magnetometer rotation
+ *
+ * This parameter defines the rotation of the external magnetometer relative
+ * to the platform (not relative to the FMU).
+ * See SENS_BOARD_ROT for possible values.
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
 
+
 /**
  * RC Channel 1 Minimum
  *
@@ -367,20 +502,61 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 #endif
-PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
+/**
+ * DSM binding trigger.
+ *
+ * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind
+ *
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_DSM_BIND, -1);
+
+
+/**
+ * Scaling factor for battery voltage sensor on PX4IO.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+/**
+ * Scaling factor for battery voltage sensor on FMU v2.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#elif CONFIG_ARCH_BOARD_AEROCORE
+/**
+ * Scaling factor for battery voltage sensor on AeroCore.
+ *
+ * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f);
 #else
-/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
-/* PX4IOAR: 0.00838095238 */
-/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
-/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
+/**
+ * Scaling factor for battery voltage sensor on FMU v1.
+ *
+ * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659
+ * FMUv1 with PX4IO: 0.00459340659
+ * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
+
+/**
+ * Scaling factor for battery current sensor.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
+
 /**
  * Roll control channel mapping.
  *
@@ -408,6 +584,20 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 
 /**
+ * Failsafe channel mapping.
+ *
+ * The RC mapping index indicates which channel is used for failsafe
+ * If 0, whichever channel is mapped to throttle is used
+ * otherwise the value indicates the specific rc channel to use
+ *
+ * @min 0
+ * @max 18
+ *
+ *
+ */
+PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0);  //Default to throttle function
+
+/**
  * Throttle control channel mapping.
  *
  * The channel index (starting from 1 for channel 1) indicates
@@ -446,22 +636,213 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
  * @group Radio Calibration
  */
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
+
+/**
+ * Return switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
-//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+/**
+ * Posctl switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
+
+/**
+ * Loiter switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
+
+/**
+ * Acro switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
 
+/**
+ * Offboard switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+
+/**
+ * Flaps channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera pitch */
+/**
+ * Auxiliary switch 1 channel mapping.
+ *
+ * Default function: Camera pitch
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);
+
+/**
+ * Auxiliary switch 2 channel mapping.
+ *
+ * Default function: Camera roll
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
-PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera azimuth / yaw */
 
-PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
-PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
-PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
+/**
+ * Auxiliary switch 3 channel mapping.
+ *
+ * Default function: Camera azimuth / yaw
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
+
+
+/**
+ * Failsafe channel PWM threshold.
+ *
+ * @min 800
+ * @max 2200
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
+
+/**
+ * Threshold for selecting assist mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
+
+/**
+ * Threshold for selecting auto mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
+
+/**
+ * Threshold for selecting posctl mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f);
+
+/**
+ * Threshold for selecting return to launch mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f);
+
+/**
+ * Threshold for selecting loiter mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f);
+
+/**
+ * Threshold for selecting acro mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
+
 
-PARAM_DEFINE_INT32(RC_FS_CH, 0);	/**< RC failsafe channel, 0 = disable */
-PARAM_DEFINE_INT32(RC_FS_MODE, 0);	/**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 800);	/**< RC failsafe PWM threshold */
+/**
+ * Threshold for selecting offboard mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b50a694eb..6bafb9ba6 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.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
@@ -37,6 +36,8 @@
  * Sensor readout process.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -125,8 +126,14 @@
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define ADC_BATTERY_VOLTAGE_CHANNEL	10
+#define ADC_BATTERY_CURRENT_CHANNEL	-1
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	-1
+#endif
+
 #define BATT_V_LOWPASS 0.001f
-#define BATT_V_IGNORE_THRESHOLD 3.5f
+#define BATT_V_IGNORE_THRESHOLD 4.8f
 
 /**
  * HACK - true temperature is much less than indicated temperature in baro,
@@ -134,7 +141,7 @@
  */
 #define PCB_TEMP_ESTIMATE_DEG 5.0f
 
-#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
+#define STICK_ON_OFF_LIMIT 0.75f
 
 /**
  * Sensor app start / stop handling function
@@ -166,7 +173,16 @@ public:
 private:
 	static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
 
-	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
+	/**
+	 * Get and limit value for specified RC function. Returns NAN if not mapped.
+	 */
+	float		get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
+
+	/**
+	 * Get switch position for specified function.
+	 */
+	switch_pos_t	get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+	switch_pos_t	get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
 
 	/**
 	 * Gather and publish RC input data.
@@ -210,10 +226,10 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
-	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
-
+	
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -232,22 +248,25 @@ private:
 		float accel_offset[3];
 		float accel_scale[3];
 		float diff_pres_offset_pa;
-		float diff_pres_analog_enabled;
+		float diff_pres_analog_scale;
 
 		int board_rotation;
 		int external_mag_rotation;
+		
+		float board_offset[3];
 
 		int rc_map_roll;
 		int rc_map_pitch;
 		int rc_map_yaw;
 		int rc_map_throttle;
+		int rc_map_failsafe;
 
 		int rc_map_mode_sw;
 		int rc_map_return_sw;
-		int rc_map_assisted_sw;
-		int rc_map_mission_sw;
-
-//		int rc_map_offboard_ctrl_mode_sw;
+		int rc_map_posctl_sw;
+		int rc_map_loiter_sw;
+		int rc_map_acro_sw;
+		int rc_map_offboard_sw;
 
 		int rc_map_flaps;
 
@@ -257,14 +276,21 @@ private:
 		int rc_map_aux4;
 		int rc_map_aux5;
 
-		float rc_scale_roll;
-		float rc_scale_pitch;
-		float rc_scale_yaw;
-		float rc_scale_flaps;
-
-		int rc_fs_ch;
-		int rc_fs_mode;
-		float rc_fs_thr;
+		int32_t rc_fails_thr;
+		float rc_assist_th;
+		float rc_auto_th;
+		float rc_posctl_th;
+		float rc_return_th;
+		float rc_loiter_th;
+		float rc_acro_th;
+		float rc_offboard_th;
+		bool rc_assist_inv;
+		bool rc_auto_inv;
+		bool rc_posctl_inv;
+		bool rc_return_inv;
+		bool rc_loiter_inv;
+		bool rc_acro_inv;
+		bool rc_offboard_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -285,19 +311,20 @@ private:
 		param_t mag_offset[3];
 		param_t mag_scale[3];
 		param_t diff_pres_offset_pa;
-		param_t diff_pres_analog_enabled;
+		param_t diff_pres_analog_scale;
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
+		param_t rc_map_failsafe;
 
 		param_t rc_map_mode_sw;
 		param_t rc_map_return_sw;
-		param_t rc_map_assisted_sw;
-		param_t rc_map_mission_sw;
-
-//		param_t rc_map_offboard_ctrl_mode_sw;
+		param_t rc_map_posctl_sw;
+		param_t rc_map_loiter_sw;
+		param_t rc_map_acro_sw;
+		param_t rc_map_offboard_sw;
 
 		param_t rc_map_flaps;
 
@@ -307,20 +334,22 @@ private:
 		param_t rc_map_aux4;
 		param_t rc_map_aux5;
 
-		param_t rc_scale_roll;
-		param_t rc_scale_pitch;
-		param_t rc_scale_yaw;
-		param_t rc_scale_flaps;
-
-		param_t rc_fs_ch;
-		param_t rc_fs_mode;
-		param_t rc_fs_thr;
+		param_t rc_fails_thr;
+		param_t rc_assist_th;
+		param_t rc_auto_th;
+		param_t rc_posctl_th;
+		param_t rc_return_th;
+		param_t rc_loiter_th;
+		param_t rc_acro_th;
+		param_t rc_offboard_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
+		
+		param_t board_offset[3];
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -421,7 +450,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };
 
 namespace sensors
@@ -437,8 +466,6 @@ Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :
-	_rc_last_valid(0),
-
 	_fd_adc(-1),
 	_last_adc(0),
 
@@ -469,12 +496,12 @@ Sensors::Sensors() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
-	_board_rotation(3, 3),
-	_external_mag_rotation(3, 3),
 	_mag_is_external(false),
 	_battery_discharged(0),
 	_battery_current_timestamp(0)
 {
+	memset(&_rc, 0, sizeof(_rc));
+	memset(&_diff_pres, 0, sizeof(_diff_pres));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -507,6 +534,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
 	_parameter_handles.rc_map_yaw 	= param_find("RC_MAP_YAW");
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
+	_parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");
 
 	/* mandatory mode switches, mapped to channel 5 and 6 per default */
 	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
@@ -515,10 +543,10 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
-	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
-	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
-
-//	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
+	_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
+	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
+	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
+	_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -526,15 +554,15 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
 	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
 
-	_parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
-	_parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
-	_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
-	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
-
-	/* RC failsafe */
-	_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
-	_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
-	_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
+	/* RC thresholds */
+	_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
+	_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
+	_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
+	_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
+	_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
+	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
+	_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
+	_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -563,7 +591,7 @@ Sensors::Sensors() :
 
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
-	_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
+	_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 	_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
@@ -571,6 +599,11 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+	
+	/* rotation offsets */
+	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
+	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
+	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -624,7 +657,7 @@ Sensors::parameters_update()
 		if (!isfinite(tmpScaleFactor) ||
 		    (tmpRevFactor < 0.000001f) ||
 		    (tmpRevFactor > 0.2f)) {
-			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
+			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i]));
 			/* scaling factors do not make sense, lock them down */
 			_parameters.scaling_factor[i] = 0.0f;
 			rc_valid = false;
@@ -635,62 +668,88 @@ Sensors::parameters_update()
 	}
 
 	/* handle wrong values */
-	if (!rc_valid)
+	if (!rc_valid) {
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
+	}
+
+	const char *paramerr = "FAIL PARM LOAD";
 
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
-		warnx("Failed getting roll chan index");
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
-		warnx("Failed getting pitch chan index");
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
-		warnx("Failed getting yaw chan index");
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
-		warnx("Failed getting throttle chan index");
+		warnx("%s", paramerr);
+	}
+
+	if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
-		warnx("Failed getting mode sw chan index");
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
-		warnx("Failed getting return sw chan index");
+		warnx("%s", paramerr);
 	}
 
-	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
-		warnx("Failed getting assisted sw chan index");
+	if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
+		warnx("%s", paramerr);
 	}
 
-	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
-		warnx("Failed getting mission sw chan index");
+	if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
+		warnx("%s", paramerr);
 	}
 
-	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("Failed getting flaps chan index");
+	if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
+	if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
+		warnx("%s", paramerr);
 	}
 
-//	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
-//		warnx("Failed getting offboard control mode sw chan index");
-//	}
+	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
+		warnx("%s", paramerr);
+	}
 
 	param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
 	param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
 	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
 	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
 	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
-	param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
-	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
-	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
-	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
-	param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
-	param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
-	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
+	param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
+	param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
+	_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
+	_parameters.rc_assist_th = fabs(_parameters.rc_assist_th);
+	param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
+	_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
+	_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
+	param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
+	_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
+	_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
+	param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th));
+	_parameters.rc_return_inv = (_parameters.rc_return_th < 0);
+	_parameters.rc_return_th = fabs(_parameters.rc_return_th);
+	param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
+	_parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0);
+	_parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th);
+	param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th));
+	_parameters.rc_acro_inv = (_parameters.rc_acro_th < 0);
+	_parameters.rc_acro_th = fabs(_parameters.rc_acro_th);
+	param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th));
+	_parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0);
+	_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -700,13 +759,13 @@ Sensors::parameters_update()
 
 	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
-	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
-	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
+	_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
+	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
+	_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
+	_rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
-//	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
-
 	_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
 	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
 	_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
@@ -740,16 +799,16 @@ Sensors::parameters_update()
 
 	/* Airspeed offset */
 	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
-	param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
+	param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
-		warnx("Failed updating voltage scaling param");
+		warnx("%s", paramerr);
 	}
 
 	/* scaling of ADC ticks to battery current */
 	if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
-		warnx("Failed updating current scaling param");
+		warnx("%s", paramerr);
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -757,6 +816,18 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+	
+	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
+	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
+	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
+	
+	/** fine tune board offset on parameter update **/
+	math::Matrix<3, 3> board_rotation_offset; 
+	board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
+	
+	_board_rotation = _board_rotation * board_rotation_offset;
 
 	return OK;
 }
@@ -784,7 +855,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -793,7 +864,7 @@ Sensors::accel_init()
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
 #else
-#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE
 
 #endif
 
@@ -819,12 +890,14 @@ Sensors::gyro_init()
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 		/* set the gyro internal sampling rate up to at least 1000Hz */
-		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
+		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) {
 			ioctl(fd, GYROIOCSSAMPLERATE, 800);
+		}
 
 		/* set the driver to poll at 1000Hz */
-		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
+		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) {
 			ioctl(fd, SENSORIOCSPOLLRATE, 800);
+		}
 
 #else
 
@@ -879,12 +952,15 @@ Sensors::mag_init()
 
 	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
 
-	if (ret < 0)
+	if (ret < 0) {
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
-	else if (ret == 1)
+
+	} else if (ret == 1) {
 		_mag_is_external = true;
-	else
+
+	} else {
 		_mag_is_external = false;
+	}
 
 	close(fd);
 }
@@ -930,7 +1006,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
@@ -941,7 +1017,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		raw.accelerometer_raw[1] = accel_report.y_raw;
 		raw.accelerometer_raw[2] = accel_report.z_raw;
 
-		raw.accelerometer_counter++;
+		raw.accelerometer_timestamp = accel_report.timestamp;
 	}
 }
 
@@ -956,7 +1032,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
@@ -967,7 +1043,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		raw.gyro_raw[1] = gyro_report.y_raw;
 		raw.gyro_raw[2] = gyro_report.z_raw;
 
-		raw.gyro_counter++;
+		raw.timestamp = gyro_report.timestamp;
 	}
 }
 
@@ -982,12 +1058,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
-		if (_mag_is_external)
+		if (_mag_is_external) {
 			vect = _external_mag_rotation * vect;
-		else
+
+		} else {
 			vect = _board_rotation * vect;
+		}
 
 		raw.magnetometer_ga[0] = vect(0);
 		raw.magnetometer_ga[1] = vect(1);
@@ -997,7 +1075,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 		raw.magnetometer_raw[1] = mag_report.y_raw;
 		raw.magnetometer_raw[2] = mag_report.z_raw;
 
-		raw.magnetometer_counter++;
+		raw.magnetometer_timestamp = mag_report.timestamp;
 	}
 }
 
@@ -1015,7 +1093,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 		raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
 		raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
 
-		raw.baro_counter++;
+		raw.baro_timestamp = _barometer.timestamp;
 	}
 }
 
@@ -1029,11 +1107,16 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
 		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
-		raw.differential_pressure_counter++;
+		raw.differential_pressure_timestamp = _diff_pres.timestamp;
+		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
-		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
-		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
-					      raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+
+		_airspeed.timestamp = _diff_pres.timestamp;
+		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
+		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub > 0) {
@@ -1100,8 +1183,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.gyro_scale[2],
 		};
 
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) {
 			warn("WARNING: failed to set scale / offsets for gyro");
+		}
 
 		close(fd);
 
@@ -1115,8 +1199,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.accel_scale[2],
 		};
 
-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
+		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) {
 			warn("WARNING: failed to set scale / offsets for accel");
+		}
 
 		close(fd);
 
@@ -1130,8 +1215,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.mag_scale[2],
 		};
 
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
+		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) {
 			warn("WARNING: failed to set scale / offsets for mag");
+		}
 
 		close(fd);
 
@@ -1145,15 +1231,17 @@ Sensors::parameter_update_poll(bool forced)
 				1.0f,
 			};
 
-			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
+			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+			}
+
 			close(fd);
 		}
 
 #if 0
-		printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
-		printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);
-		printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100));
+		printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]);
+		printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]);
+		printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100));
 		fflush(stdout);
 		usleep(5000);
 #endif
@@ -1164,22 +1252,27 @@ void
 Sensors::adc_poll(struct sensor_combined_s &raw)
 {
 	/* only read if publishing */
-	if (!_publishing)
+	if (!_publishing) {
 		return;
+	}
 
 	hrt_abstime t = hrt_absolute_time();
+
 	/* rate limit to 100 Hz */
 	if (t - _last_adc >= 10000) {
-		/* make space for a maximum of eight channels */
-		struct adc_msg_s buf_adc[8];
+		/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
+		struct adc_msg_s buf_adc[12];
 		/* read all channels available */
 		int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
 
 		if (ret >= (int)sizeof(buf_adc[0])) {
-			for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
+
+			/* Read add channels we got */
+			for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
 				/* Save raw voltage values */
-				if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
+				if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) {
 					raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
+					raw.adc_mapping[i] = buf_adc[i].am_channel;
 				}
 
 				/* look for specific channels and process the raw voltage to measurement data */
@@ -1189,6 +1282,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 					if (voltage > BATT_V_IGNORE_THRESHOLD) {
 						_battery_status.voltage_v = voltage;
+
 						/* one-time initialization of low-pass value to avoid long init delays */
 						if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
 							_battery_status.voltage_filtered_v = voltage;
@@ -1207,38 +1301,46 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					/* handle current only if voltage is valid */
 					if (_battery_status.voltage_v > 0.0f) {
 						float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
+
 						/* check measured current value */
 						if (current >= 0.0f) {
 							_battery_status.timestamp = t;
 							_battery_status.current_a = current;
+
 							if (_battery_current_timestamp != 0) {
 								/* initialize discharged value */
-								if (_battery_status.discharged_mah < 0.0f)
+								if (_battery_status.discharged_mah < 0.0f) {
 									_battery_status.discharged_mah = 0.0f;
+								}
+
 								_battery_discharged += current * (t - _battery_current_timestamp);
 								_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
 							}
 						}
 					}
+
 					_battery_current_timestamp = t;
 
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 
 					/* calculate airspeed, raw is the difference from */
-					float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f;  //V_ref/4096 * (voltage divider factor)
+					float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f;  // V_ref/4096 * (voltage divider factor)
 
 					/**
 					 * The voltage divider pulls the signal down, only act on
 					 * a valid voltage from a connected sensor. Also assume a non-
 					 * zero offset from the sensor if its connected.
 					 */
-					if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) {
+					if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
 
-						float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
+						float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
+						float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
 
 						_diff_pres.timestamp = t;
 						_diff_pres.differential_pressure_pa = diff_pres_pa;
-						_diff_pres.voltage = voltage;
+						_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
+						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
+						_diff_pres.temperature = -1000.0f;
 
 						/* announce the airspeed if needed, just publish else */
 						if (_diff_pres_pub > 0) {
@@ -1250,8 +1352,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					}
 				}
 			}
+
 			_last_adc = t;
-			if (_battery_status.voltage_v > 0.0f) {
+
+			if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
 				/* announce the battery status if needed, just publish else */
 				if (_battery_pub > 0) {
 					orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
@@ -1264,6 +1368,66 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 	}
 }
 
+float
+Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
+{
+	if (_rc.function[func] >= 0) {
+		float value = _rc.channels[_rc.function[func]];
+
+		if (value < min_value) {
+			return min_value;
+
+		} else if (value > max_value) {
+			return max_value;
+
+		} else {
+			return value;
+		}
+
+	} else {
+		return 0.0f;
+	}
+}
+
+switch_pos_t
+Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
+{
+	if (_rc.function[func] >= 0) {
+		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
+
+		if (on_inv ? value < on_th : value > on_th) {
+			return SWITCH_POS_ON;
+
+		} else if (mid_inv ? value < mid_th : value > mid_th) {
+			return SWITCH_POS_MIDDLE;
+
+		} else {
+			return SWITCH_POS_OFF;
+		}
+
+	} else {
+		return SWITCH_POS_NONE;
+	}
+}
+
+switch_pos_t
+Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
+{
+	if (_rc.function[func] >= 0) {
+		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
+
+		if (on_inv ? value < on_th : value > on_th) {
+			return SWITCH_POS_ON;
+
+		} else {
+			return SWITCH_POS_OFF;
+		}
+
+	} else {
+		return SWITCH_POS_NONE;
+	}
+}
+
 void
 Sensors::rc_poll()
 {
@@ -1272,70 +1436,58 @@ Sensors::rc_poll()
 
 	if (rc_updated) {
 		/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
-		struct rc_input_values	rc_input;
+		struct rc_input_values rc_input;
 
 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
 
-		if (rc_input.rc_lost)
-			return;
-
-		struct manual_control_setpoint_s manual_control;
-		struct actuator_controls_s actuator_group_3;
-
-		/* initialize to default values */
-		manual_control.roll = NAN;
-		manual_control.pitch = NAN;
-		manual_control.yaw = NAN;
-		manual_control.throttle = NAN;
-
-		manual_control.mode_switch = NAN;
-		manual_control.return_switch = NAN;
-		manual_control.assisted_switch = NAN;
-		manual_control.mission_switch = NAN;
-//		manual_control.auto_offboard_input_switch = NAN;
-
-		manual_control.flaps = NAN;
-		manual_control.aux1 = NAN;
-		manual_control.aux2 = NAN;
-		manual_control.aux3 = NAN;
-		manual_control.aux4 = NAN;
-		manual_control.aux5 = NAN;
-
-		/* require at least four channels to consider the signal valid */
-		if (rc_input.channel_count < 4)
-			return;
-
-		/* failsafe check */
-		if (_parameters.rc_fs_ch != 0) {
-			if (_parameters.rc_fs_mode == 0) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
-					return;
-
-			} else if (_parameters.rc_fs_mode == 1) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
-					return;
+		/* detect RC signal loss */
+		bool signal_lost;
+
+		/* check flags and require at least four channels to consider the signal valid */
+		if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) {
+			/* signal is lost or no enough channels */
+			signal_lost = true;
+
+		} else {
+			/* signal looks good */
+			signal_lost = false;
+
+			/* check failsafe */
+			int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle
+
+			if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping
+				fs_ch = _parameters.rc_map_failsafe - 1;
+			}
+
+			if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) {
+				/* failsafe configured */
+				if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) ||
+				    (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) {
+					/* failsafe triggered, signal is lost by receiver */
+					signal_lost = true;
+				}
 			}
 		}
 
 		unsigned channel_limit = rc_input.channel_count;
 
-		if (channel_limit > _rc_max_chan_count)
+		if (channel_limit > _rc_max_chan_count) {
 			channel_limit = _rc_max_chan_count;
+		}
 
-		/* we are accepting this message */
-		_rc_last_valid = rc_input.timestamp_last_signal;
-
-		/* Read out values from raw message */
+		/* read out and scale values from raw message even if signal is invalid */
 		for (unsigned int i = 0; i < channel_limit; i++) {
 
 			/*
 			 * 1) Constrain to min/max values, as later processing depends on bounds.
 			 */
-			if (rc_input.values[i] < _parameters.min[i])
+			if (rc_input.values[i] < _parameters.min[i]) {
 				rc_input.values[i] = _parameters.min[i];
+			}
 
-			if (rc_input.values[i] > _parameters.max[i])
+			if (rc_input.values[i] > _parameters.max[i]) {
 				rc_input.values[i] = _parameters.max[i];
+			}
 
 			/*
 			 * 2) Scale around the mid point differently for lower and upper range.
@@ -1354,146 +1506,96 @@ Sensors::rc_poll()
 			 * DO NOT REMOVE OR ALTER STEP 1!
 			 */
 			if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
-				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
+				_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
 
 			} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
-				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
+				_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
 
 			} else {
 				/* in the configured dead zone, output zero */
-				_rc.chan[i].scaled = 0.0f;
+				_rc.channels[i] = 0.0f;
 			}
 
-			_rc.chan[i].scaled *= _parameters.rev[i];
+			_rc.channels[i] *= _parameters.rev[i];
 
 			/* handle any parameter-induced blowups */
-			if (!isfinite(_rc.chan[i].scaled))
-				_rc.chan[i].scaled = 0.0f;
-		}
-
-		_rc.chan_count = rc_input.channel_count;
-		_rc.timestamp = rc_input.timestamp_last_signal;
-
-		manual_control.timestamp = rc_input.timestamp_last_signal;
-
-		/* roll input - rolling right is stick-wise and rotation-wise positive */
-		manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
-		/*
-		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
-		 * so reverse sign.
-		 */
-		manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
-		/* yaw input - stick right is positive and positive rotation */
-		manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
-		/* throttle input */
-		manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
-
-		if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
-
-		if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
-
-		/* scale output */
-		if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
-			manual_control.roll *= _parameters.rc_scale_roll;
-		}
-
-		if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
-			manual_control.pitch *= _parameters.rc_scale_pitch;
-		}
-
-		if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
-			manual_control.yaw *= _parameters.rc_scale_yaw;
-		}
-
-		/* flaps */
-		if (_rc.function[FLAPS] >= 0) {
-
-			manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
-
-			if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
-				manual_control.flaps *= _parameters.rc_scale_flaps;
+			if (!isfinite(_rc.channels[i])) {
+				_rc.channels[i] = 0.0f;
 			}
 		}
 
-		if (_rc.function[MODE] >= 0) {
-			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
-		}
-
-		if (_rc.function[MISSION] >= 0) {
-			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
-		}
-
-		/* land switch input */
-		if (_rc.function[RETURN] >= 0) {
-			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
-		}
-
-		/* assisted switch input */
-		if (_rc.function[ASSISTED] >= 0) {
-			manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
-		}
-
-//		if (_rc.function[OFFBOARD_MODE] >= 0) {
-//			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
-//		}
-
-		/* aux functions, only assign if valid mapping is present */
-		if (_rc.function[AUX_1] >= 0) {
-			manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
-		}
-
-		if (_rc.function[AUX_2] >= 0) {
-			manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
-		}
-
-		if (_rc.function[AUX_3] >= 0) {
-			manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
-		}
-
-		if (_rc.function[AUX_4] >= 0) {
-			manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
-		}
-
-		if (_rc.function[AUX_5] >= 0) {
-			manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
-		}
+		_rc.channel_count = rc_input.channel_count;
+		_rc.rssi = rc_input.rssi;
+		_rc.signal_lost = signal_lost;
+		_rc.timestamp = rc_input.timestamp_last_signal;
 
-		/* copy from mapped manual control to control group 3 */
-		actuator_group_3.control[0] = manual_control.roll;
-		actuator_group_3.control[1] = manual_control.pitch;
-		actuator_group_3.control[2] = manual_control.yaw;
-		actuator_group_3.control[3] = manual_control.throttle;
-		actuator_group_3.control[4] = manual_control.flaps;
-		actuator_group_3.control[5] = manual_control.aux1;
-		actuator_group_3.control[6] = manual_control.aux2;
-		actuator_group_3.control[7] = manual_control.aux3;
-
-		/* check if ready for publishing */
+		/* publish rc_channels topic even if signal is invalid, for debug */
 		if (_rc_pub > 0) {
 			orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
 
 		} else {
-			/* advertise the rc topic */
 			_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
 		}
 
-		/* check if ready for publishing */
-		if (_manual_control_pub > 0) {
-			orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
+		if (!signal_lost) {
+			struct manual_control_setpoint_s manual;
+			memset(&manual, 0 , sizeof(manual));
+
+			/* fill values in manual_control_setpoint topic only if signal is valid */
+			manual.timestamp = rc_input.timestamp_last_signal;
+
+			/* limit controls */
+			manual.y = get_rc_value(ROLL, -1.0, 1.0);
+			manual.x = get_rc_value(PITCH, -1.0, 1.0);
+			manual.r = get_rc_value(YAW, -1.0, 1.0);
+			manual.z = get_rc_value(THROTTLE, 0.0, 1.0);
+			manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
+			manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
+			manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
+			manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
+			manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
+			manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
+
+			/* mode switches */
+			manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+			manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
+			manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
+			manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+			manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
+			manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
+
+			/* publish manual_control_setpoint topic */
+			if (_manual_control_pub > 0) {
+				orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);
 
-		} else {
-			_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
-		}
+			} else {
+				_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
+			}
 
-		/* check if ready for publishing */
-		if (_actuator_group_3_pub > 0) {
-			orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
+			/* copy from mapped manual control to control group 3 */
+			struct actuator_controls_s actuator_group_3;
+			memset(&actuator_group_3, 0 , sizeof(actuator_group_3));
 
-		} else {
-			_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
+			actuator_group_3.timestamp = rc_input.timestamp_last_signal;
+
+			actuator_group_3.control[0] = manual.y;
+			actuator_group_3.control[1] = manual.x;
+			actuator_group_3.control[2] = manual.r;
+			actuator_group_3.control[3] = manual.z;
+			actuator_group_3.control[4] = manual.flaps;
+			actuator_group_3.control[5] = manual.aux1;
+			actuator_group_3.control[6] = manual.aux2;
+			actuator_group_3.control[7] = manual.aux3;
+
+			/* publish actuator_controls_3 topic */
+			if (_actuator_group_3_pub > 0) {
+				orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
+
+			} else {
+				_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
+			}
 		}
 	}
-
 }
 
 void
@@ -1570,12 +1672,10 @@ Sensors::task_main()
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		/* wait for up to 50ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
 
-		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
-			continue;
+		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -1591,8 +1691,7 @@ Sensors::task_main()
 		/* check parameters for updates */
 		parameter_update_poll();
 
-		/* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */
-		raw.timestamp = hrt_absolute_time();
+		/* the timestamp of the raw struct is updated by the gyro_poll() method */
 
 		/* copy most recent sensor data */
 		gyro_poll(raw);
@@ -1606,8 +1705,9 @@ Sensors::task_main()
 		diff_pres_poll(raw);
 
 		/* Inform other processes that new data is available to copy */
-		if (_publishing)
+		if (_publishing) {
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
+		}
 
 		/* Look for new r/c input data */
 		rc_poll();
@@ -1615,7 +1715,7 @@ Sensors::task_main()
 		perf_end(_loop_perf);
 	}
 
-	printf("[sensors] exiting.\n");
+	warnx("[sensors] exiting.");
 
 	_sensors_task = -1;
 	_exit(0);
@@ -1630,7 +1730,7 @@ Sensors::start()
 	_sensors_task = task_spawn_cmd("sensors_task",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2048,
+				       2000,
 				       (main_t)&Sensors::task_main_trampoline,
 				       nullptr);
 
@@ -1644,31 +1744,35 @@ Sensors::start()
 
 int sensors_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		errx(1, "usage: sensors {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (sensors::g_sensors != nullptr)
-			errx(0, "sensors task already running");
+		if (sensors::g_sensors != nullptr) {
+			errx(0, "already running");
+		}
 
 		sensors::g_sensors = new Sensors;
 
-		if (sensors::g_sensors == nullptr)
-			errx(1, "sensors task alloc failed");
+		if (sensors::g_sensors == nullptr) {
+			errx(1, "alloc failed");
+		}
 
 		if (OK != sensors::g_sensors->start()) {
 			delete sensors::g_sensors;
 			sensors::g_sensors = nullptr;
-			err(1, "sensors task start failed");
+			err(1, "start failed");
 		}
 
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (sensors::g_sensors == nullptr)
-			errx(1, "sensors task not running");
+		if (sensors::g_sensors == nullptr) {
+			errx(1, "not running");
+		}
 
 		delete sensors::g_sensors;
 		sensors::g_sensors = nullptr;
@@ -1677,14 +1781,13 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (sensors::g_sensors) {
-			errx(0, "task is running");
+			errx(0, "is running");
 
 		} else {
-			errx(1, "task is not running");
+			errx(1, "not running");
 		}
 	}
 
 	warnx("unrecognized command");
 	return 1;
 }
-
diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c
index ad8c2bf83..182fd15c6 100644
--- a/src/modules/systemlib/board_serial.c
+++ b/src/modules/systemlib/board_serial.c
@@ -44,11 +44,11 @@
 #include "board_config.h"
 #include "board_serial.h"
 
-int get_board_serial(char *serialid)
+int get_board_serial(uint8_t *serialid)
 {
-	const volatile unsigned *udid_ptr = (const unsigned *)UDID_START;
+	const volatile uint32_t *udid_ptr = (const uint32_t *)UDID_START;
 	union udid id;
-	val_read((unsigned *)&id, udid_ptr, sizeof(id));
+	val_read((uint32_t *)&id, udid_ptr, sizeof(id));
 
 
 	/* Copy the serial from the chips non-write memory and swap endianess */
@@ -57,4 +57,4 @@ int get_board_serial(char *serialid)
 	serialid[8] = id.data[11];   serialid[9] = id.data[10];  serialid[10] = id.data[9];  serialid[11] = id.data[8];
 
 	return 0;
-}
\ No newline at end of file
+}
diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h
index b14bb4376..873d9657b 100644
--- a/src/modules/systemlib/board_serial.h
+++ b/src/modules/systemlib/board_serial.h
@@ -44,6 +44,6 @@
 
 __BEGIN_DECLS
 
-__EXPORT int get_board_serial(char *serialid);
+__EXPORT int get_board_serial(uint8_t *serialid);
 
 __END_DECLS
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/systemlib/circuit_breaker.c
index fc658dadb..8f697749e 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/systemlib/circuit_breaker.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   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
@@ -33,69 +32,62 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_pos_control_params.h
+ * @file circuit_breaker.c
  *
- * Parameters for multirotor_pos_control
+ * Circuit breaker parameters.
+ * Analog to real aviation circuit breakers these parameters
+ * allow to disable subsystems. They are not supported as standard
+ * operation procedure and are only provided for development purposes.
+ * To ensure they are not activated accidentally, the associated
+ * parameter needs to set to the key (magic).
  */
 
 #include <systemlib/param/param.h>
+#include <systemlib/circuit_breaker.h>
 
-struct multirotor_position_control_params {
-	float takeoff_alt;
-	float takeoff_gap;
-	float thr_min;
-	float thr_max;
-	float z_p;
-	float z_d;
-	float z_vel_p;
-	float z_vel_i;
-	float z_vel_d;
-	float z_vel_max;
-	float xy_p;
-	float xy_d;
-	float xy_vel_p;
-	float xy_vel_i;
-	float xy_vel_d;
-	float xy_vel_max;
-	float tilt_max;
-
-	float rc_scale_pitch;
-	float rc_scale_roll;
-	float rc_scale_yaw;
-};
-
-struct multirotor_position_control_param_handles {
-	param_t takeoff_alt;
-	param_t takeoff_gap;
-	param_t thr_min;
-	param_t thr_max;
-	param_t z_p;
-	param_t z_d;
-	param_t z_vel_p;
-	param_t z_vel_i;
-	param_t z_vel_d;
-	param_t z_vel_max;
-	param_t xy_p;
-	param_t xy_d;
-	param_t xy_vel_p;
-	param_t xy_vel_i;
-	param_t xy_vel_d;
-	param_t xy_vel_max;
-	param_t tilt_max;
-
-	param_t rc_scale_pitch;
-	param_t rc_scale_roll;
-	param_t rc_scale_yaw;
-};
+/**
+ * Circuit breaker for power supply check
+ *
+ * Setting this parameter to 894281 will disable the power valid
+ * checks in the commander.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 894281
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
 
 /**
- * Initialize all parameter handles and values
+ * Circuit breaker for rate controller output
+ *
+ * Setting this parameter to 140253 will disable the rate
+ * controller uORB publication.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
  *
+ * @min 0
+ * @max 140253
+ * @group Circuit Breaker
  */
-int parameters_init(struct multirotor_position_control_param_handles *h);
+PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
 
 /**
- * Update all parameters
+ * Circuit breaker for IO safety
+ *
+ * Setting this parameter to 894281 will disable IO safety.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
  *
+ * @min 0
+ * @max 22027
+ * @group Circuit Breaker
  */
-int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
+PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
+
+bool circuit_breaker_enabled(const char* breaker, int32_t magic)
+{
+	int32_t val;
+	(void)param_get(param_find(breaker), &val);
+
+	return (val == magic);
+}
+
diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/modules/systemlib/circuit_breaker.h
index 399eecfa7..1175dbce8 100644
--- a/src/lib/mathlib/math/EulerAngles.hpp
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,44 +31,34 @@
  *
  ****************************************************************************/
 
-/**
- * @file Vector.h
+/*
+ * @file circuit_breaker.h
  *
- * math vector
+ * Circuit breaker functionality.
  */
 
-#pragma once
+#ifndef CIRCUIT_BREAKER_H_
+#define CIRCUIT_BREAKER_H_
 
-#include "Vector.hpp"
-
-namespace math
-{
-
-class Quaternion;
-class Dcm;
-
-class __EXPORT EulerAngles : public Vector
-{
-public:
-	EulerAngles();
-	EulerAngles(float phi, float theta, float psi);
-	EulerAngles(const Quaternion &q);
-	EulerAngles(const Dcm &dcm);
-	virtual ~EulerAngles();
-
-	// alias
-	void setPhi(float phi) { (*this)(0) = phi; }
-	void setTheta(float theta) { (*this)(1) = theta; }
-	void setPsi(float psi) { (*this)(2) = psi; }
+/* SAFETY WARNING  --  SAFETY WARNING  --  SAFETY WARNING
+ *
+ * OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE,
+ * ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS.
+ * http://pixhawk.org/dev/circuit_breakers
+ *
+ * CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE
+ * AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT.
+ */
+#define CBRK_SUPPLY_CHK_KEY	894281
+#define CBRK_RATE_CTRL_KEY	140253
+#define CBRK_IO_SAFETY_KEY	22027
 
-	// const accessors
-	const float &getPhi() const { return (*this)(0); }
-	const float &getTheta() const { return (*this)(1); }
-	const float &getPsi() const { return (*this)(2); }
+#include <stdbool.h>
 
-};
+__BEGIN_DECLS
 
-int __EXPORT eulerAnglesTest();
+__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
 
-} // math
+__END_DECLS
 
+#endif /* CIRCUIT_BREAKER_H_ */
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index ccc516f39..7aa2f3a5f 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT
 
 __EXPORT struct system_load_s system_load;
 
-extern FAR struct _TCB *sched_gettcb(pid_t pid);
+extern FAR struct tcb_s *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index 6c0e876d1..998b5ac7d 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args)
 	fprintf(stderr, "\n");
 #elif CONFIG_ARCH_LOWPUTC
 	lowsyslog("%s: ", getprogname());
-	lowvyslog(fmt, args);
+	lowvsyslog(fmt, args);
 
 	/* convenience as many parts of NuttX use negative errno */
 	if (errcode < 0)
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 8e9c2bfcf..52ae77de5 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -63,7 +63,7 @@ struct hx_stream {
 	/* TX state */
 	int			fd;
 	bool			tx_error;
-	uint8_t			*tx_buf;
+	const uint8_t		*tx_buf;
 	unsigned		tx_resid;
 	uint32_t		tx_crc;
 	enum {
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index cce46bf5f..20b1f18ed 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -171,7 +171,6 @@ NullMixer *
 NullMixer::from_text(const char *buf, unsigned &buflen)
 {
 	NullMixer *nm = nullptr;
-	const char *end = buf + buflen;
 
 	/* enforce that the mixer ends with space or a new line */
 	for (int i = buflen - 1; i >= 0; i--) {
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 1c889a811..225570fa4 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -447,6 +447,7 @@ public:
 		QUAD_WIDE,	/**< quad in wide configuration */
 		HEX_X,		/**< hex in X configuration */
 		HEX_PLUS,	/**< hex in + configuration */
+		HEX_COX,
 		OCTA_X,
 		OCTA_PLUS,
 		OCTA_COX,
@@ -516,7 +517,7 @@ private:
 	float				_roll_scale;
 	float				_pitch_scale;
 	float				_yaw_scale;
-	float				_deadband;
+	float				_idle_speed;
 
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index a55ddf8a3..bf3428a50 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -41,6 +41,7 @@
 #include <string.h>
 #include <stdio.h>
 #include <ctype.h>
+#include <systemlib/err.h>
 
 #include "mixer_load.h"
 
@@ -52,7 +53,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 	/* open the mixer definition file */
 	fp = fopen(fname, "r");
 	if (fp == NULL) {
-		return 1;
+		warnx("file not found");
+		return -1;
 	}
 
 	/* read valid lines from the file into a buffer */
@@ -88,7 +90,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 
 		/* if the line is too long to fit in the buffer, bail */
 		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
-			return 1;
+			warnx("line too long");
+			return -1;
 		}
 
 		/* add the line to the buffer */
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index bf77795d5..4b22a46d0 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -67,6 +67,11 @@
 namespace
 {
 
+float constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
 /*
  * These tables automatically generated by multi_tables - do not edit.
  */
@@ -110,6 +115,14 @@ const MultirotorMixer::Rotor _config_hex_plus[] = {
 	{  0.866025,  0.500000,  1.00 },
 	{ -0.866025, -0.500000, -1.00 },
 };
+const MultirotorMixer::Rotor _config_hex_cox[] = {
+	{ -0.866025,  0.500000, -1.00 },
+	{ -0.866025,  0.500000,  1.00 },
+	{ -0.000000, -1.000000, -1.00 },
+	{ -0.000000, -1.000000,  1.00 },
+	{  0.866025,  0.500000, -1.00 },
+	{  0.866025,  0.500000,  1.00 },
+};
 const MultirotorMixer::Rotor _config_octa_x[] = {
 	{ -0.382683,  0.923880, -1.00 },
 	{  0.382683, -0.923880, -1.00 },
@@ -147,6 +160,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_wide[0],
 	&_config_hex_x[0],
 	&_config_hex_plus[0],
+	&_config_hex_cox[0],
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
 	&_config_octa_cox[0],
@@ -158,6 +172,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_wide */
 	6, /* hex_x */
 	6, /* hex_plus */
+	6, /* hex_cox */
 	8, /* octa_x */
 	8, /* octa_plus */
 	8, /* octa_cox */
@@ -171,12 +186,12 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 				 float roll_scale,
 				 float pitch_scale,
 				 float yaw_scale,
-				 float deadband) :
+				 float idle_speed) :
 	Mixer(control_cb, cb_handle),
 	_roll_scale(roll_scale),
 	_pitch_scale(pitch_scale),
 	_yaw_scale(yaw_scale),
-	_deadband(-1.0f + deadband),	/* shift to output range here to avoid runtime calculation */
+	_idle_speed(-1.0f + idle_speed * 2.0f),	/* shift to output range here to avoid runtime calculation */
 	_rotor_count(_config_rotor_count[geometry]),
 	_rotors(_config_index[geometry])
 {
@@ -193,7 +208,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	char geomname[8];
 	int s[4];
 	int used;
-	const char *end = buf + buflen;
 
 	/* enforce that the mixer ends with space or a new line */
 	for (int i = buflen - 1; i >= 0; i--) {
@@ -247,6 +261,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	} else if (!strcmp(geomname, "6x")) {
 		geometry = MultirotorMixer::HEX_X;
 
+	} else if (!strcmp(geomname, "6c")) {
+		geometry = MultirotorMixer::HEX_COX;
+
 	} else if (!strcmp(geomname, "8+")) {
 		geometry = MultirotorMixer::OCTA_PLUS;
 
@@ -276,67 +293,66 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 unsigned
 MultirotorMixer::mix(float *outputs, unsigned space)
 {
-	float		roll    = get_control(0, 0) * _roll_scale;
+	float		roll    = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
 	//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
-	float		pitch   = get_control(0, 1) * _pitch_scale;
-	float		yaw     = get_control(0, 2) * _yaw_scale;
-	float		thrust  = get_control(0, 3);
+	float		pitch   = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
+	float		yaw     = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
+	float		thrust  = constrain(get_control(0, 3), 0.0f, 1.0f);
 	//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
-	float		max     = 0.0f;
-	float		fixup_scale;
+	float		min_out = 0.0f;
+	float		max_out = 0.0f;
 
-	/* use an output factor to prevent too strong control signals at low throttle */
-	float min_thrust = 0.05f;
-	float max_thrust = 1.0f;
-	float startpoint_full_control = 0.40f;
-	float output_factor;
+	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		float out = roll * _rotors[i].roll_scale +
+			    pitch * _rotors[i].pitch_scale +
+			    thrust;
 
-	/* keep roll, pitch and yaw control to 0 below min thrust */
-	if (thrust <= min_thrust) {
-		output_factor = 0.0f;
-		/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+		/* limit yaw if it causes outputs clipping */
+		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
+			yaw = -out / _rotors[i].yaw_scale;
+		}
 
-	} else if (thrust < startpoint_full_control && thrust > min_thrust) {
-		output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
-		/* and then stay at full control */
+		/* calculate min and max output values */
+		if (out < min_out) {
+			min_out = out;
+		}
+		if (out > max_out) {
+			max_out = out;
+		}
 
-	} else {
-		output_factor = max_thrust;
+		outputs[i] = out;
 	}
 
-	roll *= output_factor;
-	pitch *= output_factor;
-	yaw *= output_factor;
-
-
-	/* perform initial mix pass yielding un-bounded outputs */
-	for (unsigned i = 0; i < _rotor_count; i++) {
-		float tmp = roll  * _rotors[i].roll_scale +
-			    pitch * _rotors[i].pitch_scale +
-			    yaw   * _rotors[i].yaw_scale +
-			    thrust;
+	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
+	if (min_out < 0.0f) {
+		float scale_in = thrust / (thrust - min_out);
 
-		if (tmp > max)
-			max = tmp;
+		/* mix again with adjusted controls */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
+		}
 
-		outputs[i] = tmp;
+	} else {
+		/* roll/pitch mixed without limiting, add yaw control */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs[i] += yaw * _rotors[i].yaw_scale;
+		}
 	}
 
-	/* scale values into the -1.0 - 1.0 range */
-	if (max > 1.0f) {
-		fixup_scale = 2.0f / max;
+	/* scale down all outputs if some outputs are too large, reduce total thrust */
+	float scale_out;
+	if (max_out > 1.0f) {
+		scale_out = 1.0f / max_out;
 
 	} else {
-		fixup_scale = 2.0f;
+		scale_out = 1.0f;
 	}
 
-	for (unsigned i = 0; i < _rotor_count; i++)
-		outputs[i] = -1.0f + (outputs[i] * fixup_scale);
-
-	/* ensure outputs are out of the deadband */
-	for (unsigned i = 0; i < _rotor_count; i++)
-		if (outputs[i] < _deadband)
-			outputs[i] = _deadband;
+	/* scale outputs to range _idle_speed..1, and do final limiting */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
+	}
 
 	return _rotor_count;
 }
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
index 050bf2f47..b5698036e 100755
--- a/src/modules/systemlib/mixer/multi_tables
+++ b/src/modules/systemlib/mixer/multi_tables
@@ -52,6 +52,15 @@ set hex_plus {
 	 120 CW
 }
 
+set hex_cox {
+	  60 CW
+	  60 CCW
+	 180 CW
+	 180 CCW
+	 -60 CW
+	 -60 CCW
+}
+
 set octa_x {
 	  22.5 CW
 	-157.5 CW
@@ -85,7 +94,7 @@ set octa_cox {
 	-135 CW
 }
 
-set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox}
+set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
 
 proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
 
@@ -104,13 +113,13 @@ foreach table $tables {
 	puts "};"
 }
 
-puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {"
 foreach table $tables {
 	puts [format "\t&_config_%s\[0\]," $table]
 }
 puts "};"
 
-puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {"
 foreach table $tables {
 	upvar #0 $table angles
 	puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table]
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 3953b757d..147903aa0 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
@@ -52,5 +52,6 @@ SRCS		 = err.c \
 		   rc_check.c \
 		   otp.c \
 		   board_serial.c \
-		   pwm_limit/pwm_limit.c
+		   pwm_limit/pwm_limit.c \
+		   circuit_breaker.c
 
diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c
index 695574fdc..0548a9f7d 100644
--- a/src/modules/systemlib/otp.c
+++ b/src/modules/systemlib/otp.c
@@ -133,7 +133,7 @@ int lock_otp(void)
 
 
 // COMPLETE, BUSY, or other flash error?
-int F_GetStatus(void)
+static int F_GetStatus(void)
 {
 	int fs = F_COMPLETE;
 
diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h
index f10e129d8..273b064f0 100644
--- a/src/modules/systemlib/otp.h
+++ b/src/modules/systemlib/otp.h
@@ -125,7 +125,7 @@ struct otp_lock {
 #pragma pack(push, 1)
 union udid {
 	uint32_t	serial[3];
-	char  data[12];
+	uint8_t  data[12];
 };
 #pragma pack(pop)
 
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 2d773fd25..e44e6cdb0 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -96,8 +96,6 @@ ORB_DEFINE(parameter_update, struct parameter_update_s);
 /** parameter update topic handle */
 static orb_advert_t param_topic = -1;
 
-static sem_t param_sem = { .semcount = 1 };
-
 /** lock the parameter store */
 static void
 param_lock(void)
@@ -521,73 +519,15 @@ param_save_default(void)
 		return ERROR;
 	}
 
-	if (res == OK) {
-		res = param_export(fd, false);
+	res = param_export(fd, false);
 
-	        if (res != OK) {
-	            warnx("failed to write parameters to file: %s", filename);
-	        }
+	if (res != OK) {
+		warnx("failed to write parameters to file: %s", filename);
 	}
 
 	close(fd);
 
 	return res;
-
-#if 0
-	const char *filename_tmp = malloc(strlen(filename) + 5);
-	sprintf(filename_tmp, "%s.tmp", filename);
-
-	/* delete temp file if exist */
-	res = unlink(filename_tmp);
-
-	if (res != OK && errno == ENOENT)
-		res = OK;
-
-	if (res != OK)
-		warn("failed to delete temp file: %s", filename_tmp);
-
-	if (res == OK) {
-		/* write parameters to temp file */
-		fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL);
-
-		if (fd < 0) {
-			warn("failed to open temp file: %s", filename_tmp);
-			res = ERROR;
-		}
-
-		if (res == OK) {
-			res = param_export(fd, false);
-
-			if (res != OK)
-				warnx("failed to write parameters to file: %s", filename_tmp);
-		}
-
-		close(fd);
-	}
-
-	if (res == OK) {
-		/* delete parameters file */
-		res = unlink(filename);
-
-		if (res != OK && errno == ENOENT)
-			res = OK;
-
-		if (res != OK)
-			warn("failed to delete parameters file: %s", filename);
-	}
-
-	if (res == OK) {
-		/* rename temp file to parameters */
-		res = rename(filename_tmp, filename);
-
-		if (res != OK)
-			warn("failed to rename %s to %s", filename_tmp, filename);
-	}
-
-	free(filename_tmp);
-
-	return res;
-#endif
 }
 
 /**
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index b4ca0ed3e..d6d8284d2 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -282,12 +282,18 @@ perf_reset(perf_counter_t handle)
 void
 perf_print_counter(perf_counter_t handle)
 {
+	perf_print_counter_fd(0, handle);
+}
+
+void
+perf_print_counter_fd(int fd, perf_counter_t handle)
+{
 	if (handle == NULL)
 		return;
 
 	switch (handle->type) {
 	case PC_COUNT:
-		printf("%s: %llu events\n",
+		dprintf(fd, "%s: %llu events\n",
 		       handle->name,
 		       ((struct perf_ctr_count *)handle)->event_count);
 		break;
@@ -295,7 +301,7 @@ perf_print_counter(perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
@@ -308,7 +314,7 @@ perf_print_counter(perf_counter_t handle)
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 
-		printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,
@@ -349,12 +355,12 @@ perf_event_count(perf_counter_t handle)
 }
 
 void
-perf_print_all(void)
+perf_print_all(int fd)
 {
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
 
 	while (handle != NULL) {
-		perf_print_counter(handle);
+		perf_print_counter_fd(fd, handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
 }
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index e1e3cbe95..668d9dfdf 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -39,6 +39,8 @@
 #ifndef _SYSTEMLIB_PERF_COUNTER_H
 #define _SYSTEMLIB_PERF_COUNTER_H value
 
+#include <stdint.h>
+
 /**
  * Counter types.
  */
@@ -73,7 +75,7 @@ __EXPORT extern void		perf_free(perf_counter_t handle);
 /**
  * Count a performance event.
  *
- * This call only affects counters that take single events; PC_COUNT etc.
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
  *
  * @param handle		The handle returned from perf_alloc.
  */
@@ -119,16 +121,26 @@ __EXPORT extern void		perf_cancel(perf_counter_t handle);
 __EXPORT extern void		perf_reset(perf_counter_t handle);
 
 /**
- * Print one performance counter.
+ * Print one performance counter to stdout
  *
  * @param handle		The counter to print.
  */
 __EXPORT extern void		perf_print_counter(perf_counter_t handle);
 
 /**
+ * Print one performance counter to a fd.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ * @param handle		The counter to print.
+ */
+__EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
+
+/**
  * Print all of the performance counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
  */
-__EXPORT extern void		perf_print_all(void);
+__EXPORT extern void		perf_print_all(int fd);
 
 /**
  * Reset all of the performance counters.
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 77c952f52..45f218a5b 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -39,7 +39,7 @@
 /**
  * @file pid.c
  *
- * Implementation of generic PID control interface.
+ * Implementation of generic PID controller.
  *
  * @author Laurens Mackay <mackayl@student.ethz.ch>
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -53,24 +53,21 @@
 
 #define SIGMA 0.000001f
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-		       float limit, uint8_t mode, float dt_min)
+__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
 {
-	pid->kp = kp;
-	pid->ki = ki;
-	pid->kd = kd;
-	pid->intmax = intmax;
-	pid->limit = limit;
 	pid->mode = mode;
 	pid->dt_min = dt_min;
-	pid->count = 0.0f;
-	pid->saturated = 0.0f;
-	pid->last_output = 0.0f;
-	pid->sp = 0.0f;
-	pid->error_previous = 0.0f;
+	pid->kp = 0.0f;
+	pid->ki = 0.0f;
+	pid->kd = 0.0f;
 	pid->integral = 0.0f;
+	pid->integral_limit = 0.0f;
+	pid->output_limit = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->last_output = 0.0f;
 }
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
+
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
 {
 	int ret = 0;
 
@@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 		ret = 1;
 	}
 
-	if (isfinite(intmax)) {
-		pid->intmax = intmax;
+	if (isfinite(integral_limit)) {
+		pid->integral_limit = integral_limit;
 
 	}  else {
 		ret = 1;
 	}
 
-	if (isfinite(limit)) {
-		pid->limit = limit;
+	if (isfinite(output_limit)) {
+		pid->output_limit = output_limit;
 
 	}  else {
 		ret = 1;
@@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 	return ret;
 }
 
-//void pid_set(PID_t *pid, float sp)
-//{
-//	pid->sp = sp;
-//	pid->error_previous = 0;
-//	pid->integral = 0;
-//}
-
-/**
- *
- * @param pid
- * @param val
- * @param dt
- * @return
- */
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
 {
-	/*  error = setpoint - actual_position
-	 integral = integral + (error*dt)
-	 derivative = (error - previous_error)/dt
-	 output = (Kp*error) + (Ki*integral) + (Kd*derivative)
-	 previous_error = error
-	 wait(dt)
-	 goto start
-	 */
-
 	if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
 		return pid->last_output;
 	}
 
 	float i, d;
-	pid->sp = sp;
 
-	// Calculated current error value
-	float error = pid->sp - val;
+	/* current error value */
+	float error = sp - val;
 
-	// Calculate or measured current error derivative
+	/* current error derivative */
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
 		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
 		pid->error_previous = error;
@@ -167,39 +140,34 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
-	if (pid->ki > 0.0f) {
+	/* calculate PD output */
+	float output = (error * pid->kp) + (d * pid->kd);
+
+	if (pid->ki > SIGMA) {
 		// Calculate the error integral and check for saturation
 		i = pid->integral + (error * dt);
 
-		if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
-		    fabsf(i) > pid->intmax) {
-			i = pid->integral;		// If saturated then do not update integral value
-			pid->saturated = 1;
-
-		} else {
-			if (!isfinite(i)) {
-				i = 0.0f;
+		/* check for saturation */
+		if (isfinite(i)) {
+			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
+			    fabsf(i) <= pid->integral_limit) {
+				/* not saturated, use new integral value */
+				pid->integral = i;
 			}
-
-			pid->integral = i;
-			pid->saturated = 0;
 		}
 
-	} else {
-		i = 0.0f;
-		pid->saturated = 0;
+		/* add I component to output */
+		output += pid->integral * pid->ki;
 	}
 
-	// Calculate the output.  Limit output magnitude to pid->limit
-	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
-
+	/* limit output */
 	if (isfinite(output)) {
-		if (pid->limit > SIGMA) {
-			if (output > pid->limit) {
-				output = pid->limit;
+		if (pid->output_limit > SIGMA) {
+			if (output > pid->output_limit) {
+				output = pid->output_limit;
 
-			} else if (output < -pid->limit) {
-				output = -pid->limit;
+			} else if (output < -pid->output_limit) {
+				output = -pid->output_limit;
 			}
 		}
 
@@ -212,5 +180,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 __EXPORT void pid_reset_integral(PID_t *pid)
 {
-	pid->integral = 0;
+	pid->integral = 0.0f;
 }
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index eca228464..e8b1aac4f 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -39,7 +39,7 @@
 /**
  * @file pid.h
  *
- * Definition of generic PID control interface.
+ * Definition of generic PID controller.
  *
  * @author Laurens Mackay <mackayl@student.ethz.ch>
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -55,38 +55,35 @@
 
 __BEGIN_DECLS
 
-/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
- * val_dot in pid_calculate() will be ignored */
-#define PID_MODE_DERIVATIV_CALC	0
-/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
- * val_dot in pid_calculate() will be ignored */
-#define PID_MODE_DERIVATIV_CALC_NO_SP	1
-/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-#define PID_MODE_DERIVATIV_SET	2
-// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
-#define PID_MODE_DERIVATIV_NONE 9
+typedef enum PID_MODE {
+	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
+	PID_MODE_DERIVATIV_NONE = 0,
+	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
+	 * val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC,
+	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
+	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC_NO_SP,
+	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
+	PID_MODE_DERIVATIV_SET
+} pid_mode_t;
 
 typedef struct {
+	pid_mode_t mode;
+	float dt_min;
 	float kp;
 	float ki;
 	float kd;
-	float intmax;
-	float sp;
 	float integral;
+	float integral_limit;
+	float output_limit;
 	float error_previous;
 	float last_output;
-	float limit;
-	float dt_min;
-	uint8_t mode;
-	uint8_t count;
-	uint8_t saturated;
 } PID_t;
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
-//void pid_set(PID_t *pid, float sp);
+__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
-
 __EXPORT void pid_reset_integral(PID_t *pid);
 
 __END_DECLS
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index 190b315f1..c733a53d7 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -97,7 +97,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 	}
 
 	unsigned progress;
-	uint16_t temp_pwm;
 
 	/* then set effective_pwm based on state */
 	switch (limit->state) {
@@ -136,12 +135,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 					}
 
 					effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
+
+					/* last line of defense against invalid inputs */
+					if (effective_pwm[i] < ramp_min_pwm) {
+						effective_pwm[i] = ramp_min_pwm;
+					} else if (effective_pwm[i] > max_pwm[i]) {
+						effective_pwm[i] = max_pwm[i];
+					}
 				}
 			}
 			break;
 		case PWM_LIMIT_STATE_ON:
 			for (unsigned i=0; i<num_channels; i++) {
 				effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+
+				/* last line of defense against invalid inputs */
+				if (effective_pwm[i] < min_pwm[i]) {
+					effective_pwm[i] = min_pwm[i];
+				} else if (effective_pwm[i] > max_pwm[i]) {
+					effective_pwm[i] = max_pwm[i];
+				}
 			}
 			break;
 		default:
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index 21e15ec56..b35b333af 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -42,6 +42,7 @@
 #include <stdio.h>
 #include <fcntl.h>
 
+#include <systemlib/err.h>
 #include <systemlib/rc_check.h>
 #include <systemlib/param/param.h>
 #include <mavlink/mavlink_log.h>
@@ -98,32 +99,32 @@ int rc_calibration_check(int mavlink_fd) {
 		/* assert min..center..max ordering */
 		if (param_min < 500) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MIN < 500", i+1);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 		if (param_max > 2500) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MAX > 2500", i+1);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 		if (param_trim < param_min) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 		if (param_trim > param_max) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 
 		/* assert deadzone is sane */
 		if (param_dz > 500) {
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_DZ > 500", i+1);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 			count++;
@@ -139,8 +140,8 @@ int rc_calibration_check(int mavlink_fd) {
 
 		/* sanity checks pass, enable channel */
 		if (count) {
-			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
-			warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			mavlink_log_critical(mavlink_fd, "#audio ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
 			usleep(100000);
 		}
 
diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h
new file mode 100644
index 000000000..e6011fdef
--- /dev/null
+++ b/src/modules/systemlib/state_table.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   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 state_table.h
+ *
+ * Finite-State-Machine helper class for state table
+ * @author: Julian Oes <julian@oes.ch>
+ */
+
+#ifndef __SYSTEMLIB_STATE_TABLE_H
+#define __SYSTEMLIB_STATE_TABLE_H
+
+class StateTable
+{
+public:
+	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;
+
+		/* accept new state */
+		myState = t->nextState;
+
+		/*  */
+		(this->*(t->action))();
+	}
+	void doNothing() {
+		return;
+	}
+protected:
+	unsigned myState;
+private:
+	Tran const *myTable;
+	unsigned myNsignals;
+	unsigned myNstates;
+};
+
+#endif
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 75be090f8..702e435ac 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -40,8 +40,45 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
-// Auto-start script with index #n
+/**
+ * Auto-start script index.
+ *
+ * Defines the auto-start script used to bootstrap the system.
+ *
+ * @group System
+ */
 PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
 
-// Automatically configure default values
+/**
+ * Automatically configure default values.
+ *
+ * Set to 1 to set platform-specific parameters to their default
+ * values on next system startup. 
+ *
+ * @min 0
+ * @max 1
+ * @group System
+ */
 PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
+
+/**
+* Set usage of IO board
+*
+* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
+*
+* @min 0
+* @max 1
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_USE_IO, 1);
+
+/**
+* Set restart type
+*
+* Set by px4io to indicate type of restart
+*
+* @min 0
+* @max 2
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 57a751e1c..90d8dd77c 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -54,6 +54,9 @@
 
 #include "systemlib.h"
 
+// Didn't seem right to include up_internal.h, so direct extern instead.
+extern void up_systemreset(void) noreturn_function;
+
 void
 systemreset(bool to_bootloader)
 {
@@ -64,6 +67,9 @@ systemreset(bool to_bootloader)
 		*(uint32_t *)0x40002850 = 0xb007b007;
 	}
 	up_systemreset();
+
+	/* lock up here */
+	while(true);
 }
 
 static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
new file mode 100644
index 000000000..cd0b30dd6
--- /dev/null
+++ b/src/modules/uORB/Publication.cpp
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Publication.cpp
+ *
+ */
+
+#include "Publication.hpp"
+#include "topics/vehicle_attitude.h"
+#include "topics/vehicle_local_position.h"
+#include "topics/vehicle_global_position.h"
+#include "topics/debug_key_value.h"
+#include "topics/actuator_controls.h"
+#include "topics/vehicle_global_velocity_setpoint.h"
+#include "topics/vehicle_attitude_setpoint.h"
+#include "topics/vehicle_rates_setpoint.h"
+#include "topics/actuator_outputs.h"
+#include "topics/encoders.h"
+#include "topics/tecs_status.h"
+
+namespace uORB {
+
+template<class T>
+Publication<T>::Publication(
+	List<PublicationBase *> * list,
+	const struct orb_metadata *meta) :
+	T(), // initialize data structure to zero
+	PublicationBase(list, meta) {
+}
+
+template<class T>
+Publication<T>::~Publication() {}
+
+template<class T>
+void * Publication<T>::getDataVoidPtr() {
+	return (void *)(T *)(this);
+}
+
+template class __EXPORT Publication<vehicle_attitude_s>;
+template class __EXPORT Publication<vehicle_local_position_s>;
+template class __EXPORT Publication<vehicle_global_position_s>;
+template class __EXPORT Publication<debug_key_value_s>;
+template class __EXPORT Publication<actuator_controls_s>;
+template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
+template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
+template class __EXPORT Publication<vehicle_rates_setpoint_s>;
+template class __EXPORT Publication<actuator_outputs_s>;
+template class __EXPORT Publication<encoders_s>;
+template class __EXPORT Publication<tecs_status_s>;
+
+}
diff --git a/src/modules/controllib/uorb/UOrbPublication.hpp b/src/modules/uORB/Publication.hpp
index 6f1f3fc1c..8650b3df8 100644
--- a/src/modules/controllib/uorb/UOrbPublication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -32,32 +32,29 @@
  ****************************************************************************/
 
 /**
- * @file UOrbPublication.h
+ * @file Publication.h
  *
  */
 
 #pragma once
 
 #include <uORB/uORB.h>
-#include "../block/Block.hpp"
-#include "../block/List.hpp"
+#include <containers/List.hpp>
 
 
-namespace control
+namespace uORB
 {
 
-class Block;
-
 /**
  * Base publication warapper class, used in list traversal
  * of various publications.
  */
-class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
+class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
 {
 public:
 
-	UOrbPublicationBase(
-		List<UOrbPublicationBase *> * list,
+	PublicationBase(
+		List<PublicationBase *> * list,
 		const struct orb_metadata *meta) :
 		_meta(meta),
 		_handle(-1) {
@@ -71,7 +68,7 @@ public:
 		}
 	}
 	virtual void *getDataVoidPtr() = 0;
-	virtual ~UOrbPublicationBase() {
+	virtual ~PublicationBase() {
 		orb_unsubscribe(getHandle());
 	}
 	const struct orb_metadata *getMeta() { return _meta; }
@@ -83,12 +80,12 @@ protected:
 };
 
 /**
- * UOrb Publication wrapper class
+ * Publication wrapper class
  */
 template<class T>
-class UOrbPublication :
+class Publication :
 	public T, // this must be first!
-	public UOrbPublicationBase
+	public PublicationBase
 {
 public:
 	/**
@@ -98,13 +95,9 @@ public:
 	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
 	 *			for the topic.
 	 */
-	UOrbPublication(
-		List<UOrbPublicationBase *> * list,
-		const struct orb_metadata *meta) :
-		T(), // initialize data structure to zero
-		UOrbPublicationBase(list, meta) {
-	}
-	virtual ~UOrbPublication() {}
+	Publication(List<PublicationBase *> * list,
+		const struct orb_metadata *meta);
+	virtual ~Publication();
 	/*
 	 * XXX
 	 * This function gets the T struct, assuming
@@ -112,7 +105,7 @@ public:
 	 * should use dynamic cast, but doesn't
 	 * seem to be available
 	 */
-	void *getDataVoidPtr() { return (void *)(T *)(this); }
+	void *getDataVoidPtr();
 };
 
-} // namespace control
+} // namespace uORB
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
new file mode 100644
index 000000000..44b6debc7
--- /dev/null
+++ b/src/modules/uORB/Subscription.cpp
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Subscription.cpp
+ *
+ */
+
+#include "Subscription.hpp"
+#include "topics/parameter_update.h"
+#include "topics/actuator_controls.h"
+#include "topics/vehicle_gps_position.h"
+#include "topics/satellite_info.h"
+#include "topics/sensor_combined.h"
+#include "topics/vehicle_attitude.h"
+#include "topics/vehicle_global_position.h"
+#include "topics/encoders.h"
+#include "topics/position_setpoint_triplet.h"
+#include "topics/vehicle_status.h"
+#include "topics/manual_control_setpoint.h"
+#include "topics/vehicle_local_position_setpoint.h"
+#include "topics/vehicle_local_position.h"
+#include "topics/vehicle_attitude_setpoint.h"
+#include "topics/vehicle_rates_setpoint.h"
+
+namespace uORB
+{
+
+bool __EXPORT SubscriptionBase::updated()
+{
+	bool isUpdated = false;
+	orb_check(_handle, &isUpdated);
+	return isUpdated;
+}
+
+template<class T>
+Subscription<T>::Subscription(
+	List<SubscriptionBase *> * list,
+	const struct orb_metadata *meta, unsigned interval) :
+	T(), // initialize data structure to zero
+	SubscriptionBase(list, meta) {
+	setHandle(orb_subscribe(getMeta()));
+	orb_set_interval(getHandle(), interval);
+}
+
+template<class T>
+Subscription<T>::~Subscription() {}
+
+template<class T>
+void * Subscription<T>::getDataVoidPtr() {
+	return (void *)(T *)(this);
+}
+
+template<class T>
+T Subscription<T>::getData() {
+	return T(*this);
+}
+
+template class __EXPORT Subscription<parameter_update_s>;
+template class __EXPORT Subscription<actuator_controls_s>;
+template class __EXPORT Subscription<vehicle_gps_position_s>;
+template class __EXPORT Subscription<satellite_info_s>;
+template class __EXPORT Subscription<sensor_combined_s>;
+template class __EXPORT Subscription<vehicle_attitude_s>;
+template class __EXPORT Subscription<vehicle_global_position_s>;
+template class __EXPORT Subscription<encoders_s>;
+template class __EXPORT Subscription<position_setpoint_triplet_s>;
+template class __EXPORT Subscription<vehicle_status_s>;
+template class __EXPORT Subscription<manual_control_setpoint_s>;
+template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
+template class __EXPORT Subscription<vehicle_local_position_s>;
+template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
+template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
+
+} // namespace uORB
diff --git a/src/modules/controllib/uorb/UOrbSubscription.hpp b/src/modules/uORB/Subscription.hpp
index d337d89a8..34e9a83e0 100644
--- a/src/modules/controllib/uorb/UOrbSubscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -32,28 +32,25 @@
  ****************************************************************************/
 
 /**
- * @file UOrbSubscription.h
+ * @file Subscription.h
  *
  */
 
 #pragma once
 
 #include <uORB/uORB.h>
-#include "../block/Block.hpp"
-#include "../block/List.hpp"
+#include <containers/List.hpp>
 
 
-namespace control
+namespace uORB
 {
 
-class Block;
-
 /**
  * Base subscription warapper class, used in list traversal
  * of various subscriptions.
  */
-class __EXPORT UOrbSubscriptionBase :
-	public ListNode<control::UOrbSubscriptionBase *>
+class __EXPORT SubscriptionBase :
+	public ListNode<SubscriptionBase *>
 {
 public:
 // methods
@@ -64,8 +61,8 @@ public:
 	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
 	 *			for the topic.
 	 */
-	UOrbSubscriptionBase(
-		List<UOrbSubscriptionBase *> * list,
+	SubscriptionBase(
+		List<SubscriptionBase *> * list,
 		const struct orb_metadata *meta) :
 		_meta(meta),
 		_handle() {
@@ -78,7 +75,7 @@ public:
 		}
 	}
 	virtual void *getDataVoidPtr() = 0;
-	virtual ~UOrbSubscriptionBase() {
+	virtual ~SubscriptionBase() {
 		orb_unsubscribe(_handle);
 	}
 // accessors
@@ -93,12 +90,12 @@ protected:
 };
 
 /**
- * UOrb Subscription wrapper class
+ * Subscription wrapper class
  */
 template<class T>
-class __EXPORT UOrbSubscription :
+class __EXPORT Subscription :
 	public T, // this must be first!
-	public UOrbSubscriptionBase
+	public SubscriptionBase
 {
 public:
 	/**
@@ -109,19 +106,13 @@ public:
 	 *			for the topic.
 	 * @param interval  The minimum interval in milliseconds between updates
 	 */
-	UOrbSubscription(
-		List<UOrbSubscriptionBase *> * list,
-		const struct orb_metadata *meta, unsigned interval) :
-		T(), // initialize data structure to zero
-		UOrbSubscriptionBase(list, meta) {
-		setHandle(orb_subscribe(getMeta()));
-		orb_set_interval(getHandle(), interval);
-	}
-
+	Subscription(
+		List<SubscriptionBase *> * list,
+		const struct orb_metadata *meta, unsigned interval);
 	/**
 	 * Deconstructor
 	 */
-	virtual ~UOrbSubscription() {}
+	virtual ~Subscription();
 
 	/*
 	 * XXX
@@ -130,8 +121,8 @@ public:
 	 * should use dynamic cast, but doesn't
 	 * seem to be available
 	 */
-	void *getDataVoidPtr() { return (void *)(T *)(this); }
-	T getData() { return T(*this); }
+	void *getDataVoidPtr();
+	T getData();
 };
 
-} // namespace control
+} // namespace uORB
diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk
index 5ec31ab01..0c29101fe 100644
--- a/src/modules/uORB/module.mk
+++ b/src/modules/uORB/module.mk
@@ -41,4 +41,6 @@ MODULE_COMMAND		= uorb
 MODULE_STACKSIZE	= 4096
 
 SRCS			= uORB.cpp \
-			  objects_common.cpp
+			  objects_common.cpp \
+			  Publication.cpp \
+			  Subscription.cpp
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index c6a252b55..c5831462b 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
 #include "topics/vehicle_gps_position.h"
 ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
 
+#include "topics/satellite_info.h"
+ORB_DEFINE(satellite_info, struct satellite_info_s);
+
 #include "topics/home_position.h"
 ORB_DEFINE(home_position, struct home_position_s);
 
@@ -90,6 +93,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
 #include "topics/servorail_status.h"
 ORB_DEFINE(servorail_status, struct servorail_status_s);
 
+#include "topics/system_power.h"
+ORB_DEFINE(system_power, struct system_power_s);
+
 #include "topics/vehicle_global_position.h"
 ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 
@@ -117,17 +123,21 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
 #include "topics/vehicle_bodyframe_speed_setpoint.h"
 ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
 
-#include "topics/vehicle_global_position_setpoint.h"
-ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
-
-#include "topics/vehicle_global_position_set_triplet.h"
-ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
+#include "topics/position_setpoint_triplet.h"
+ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
 
 #include "topics/vehicle_global_velocity_setpoint.h"
 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"
+ORB_DEFINE(mission_result, struct mission_result_s);
+
+#include "topics/fence.h"
+ORB_DEFINE(fence, unsigned);
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
@@ -176,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
 #include "topics/telemetry_status.h"
-ORB_DEFINE(telemetry_status, struct telemetry_status_s);
+ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
+ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
+ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
+ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
 
 #include "topics/debug_key_value.h"
 ORB_DEFINE(debug_key_value, struct debug_key_value_s);
@@ -186,3 +199,18 @@ ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
 
 #include "topics/esc_status.h"
 ORB_DEFINE(esc_status, struct esc_status_s);
+
+#include "topics/encoders.h"
+ORB_DEFINE(encoders, struct encoders_s);
+
+#include "topics/estimator_status.h"
+ORB_DEFINE(estimator_status, struct estimator_status_report);
+
+#include "topics/vehicle_force_setpoint.h"
+ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s);
+
+#include "topics/tecs_status.h"
+ORB_DEFINE(tecs_status, struct tecs_status_s);
+
+#include "topics/wind_estimate.h"
+ORB_DEFINE(wind_estimate, struct wind_estimate_s);
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index 6e944ffee..a98d3fc3a 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -44,15 +44,25 @@
 #include <stdint.h>
 #include "../uORB.h"
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /** global 'actuator output is live' control. */
 struct actuator_armed_s {
 
-	uint64_t	timestamp;
-	bool	armed;		/**< Set to true if system is armed */
-	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
-	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+	uint64_t	timestamp;	/**< Microseconds since system boot */
+	bool		armed;		/**< Set to true if system is armed */
+	bool		ready_to_arm;	/**< Set to true if system is ready to be armed */
+	bool		lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
 };
 
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
 ORB_DECLARE(actuator_armed);
 
 #endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
index a3da3758f..d2ee754cd 100644
--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -52,9 +52,10 @@
  * Airspeed
  */
 struct airspeed_s {
-	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
+	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
-	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
+	float		true_airspeed_m_s;		/**< true airspeed in meters per second, -1 if unknown */
+	float		air_temperature_celsius;	/**< air temperature in degrees celsius, -1000 if unknown */
 };
 
 /**
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index 5d94d4288..cd48d3cb2 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -52,12 +52,13 @@
  * Differential pressure.
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
-	uint64_t	error_count;
-	float	differential_pressure_pa;	/**< Differential pressure reading */
-	float	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
-	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
-	float		temperature;			/**< Temperature provided by sensor */
+	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
+	uint64_t	error_count;			/**< Number of errors detected by driver */
+	float	differential_pressure_pa;		/**< Differential pressure reading */
+	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
+	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
+	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */
+	float	temperature;				/**< Temperature provided by sensor, -1000.0f if unknown */
 
 };
 
diff --git a/src/modules/controllib/uorb/UOrbSubscription.cpp b/src/modules/uORB/topics/encoders.h
index 022cadd24..588c0ddb1 100644
--- a/src/modules/controllib/uorb/UOrbSubscription.cpp
+++ b/src/modules/uORB/topics/encoders.h
@@ -32,20 +32,35 @@
  ****************************************************************************/
 
 /**
- * @file UOrbSubscription.cpp
+ * @file encoders.h
+ *
+ * Encoders topic.
  *
  */
 
-#include "UOrbSubscription.hpp"
+#ifndef TOPIC_ENCODERS_H
+#define TOPIC_ENCODERS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
 
-namespace control
-{
+#define NUM_ENCODERS 4
+
+struct encoders_s {
+	uint64_t timestamp;
+	int64_t counts[NUM_ENCODERS]; // counts of encoder
+	float velocity[NUM_ENCODERS]; // counts of encoder/ second
+};
+
+/**
+ * @}
+ */
 
-bool __EXPORT UOrbSubscriptionBase::updated()
-{
-	bool isUpdated = false;
-	orb_check(_handle, &isUpdated);
-	return isUpdated;
-}
+ORB_DECLARE(encoders);
 
-} // namespace control
+#endif
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index 11332d7a7..628824efa 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -60,7 +60,7 @@
 enum ESC_VENDOR {
 	ESC_VENDOR_GENERIC = 0,					/**< generic ESC */
 	ESC_VENDOR_MIKROKOPTER,					/**< Mikrokopter */
-	ESC_VENDOR_GRAUPNER_HOTT				/**< Graupner HoTT ESC */	
+	ESC_VENDOR_GRAUPNER_HOTT				/**< Graupner HoTT ESC */
 };
 
 enum ESC_CONNECTION_TYPE {
@@ -79,16 +79,15 @@ enum ESC_CONNECTION_TYPE {
 /**
  * Electronic speed controller status.
  */
-struct esc_status_s
-{
+struct esc_status_s {
 	/* use of a counter and timestamp recommended (but not necessary) */
 
 	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 */
-	
+
 	uint8_t esc_count;			/**< number of connected ESCs */
 	enum ESC_CONNECTION_TYPE esc_connectiontype;	/**< how ESCs connected to the system */
-	
+
 	struct {
 		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
 		enum ESC_VENDOR esc_vendor;		/**< Vendor of current ESC */
diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h
new file mode 100644
index 000000000..7f26b505b
--- /dev/null
+++ b/src/modules/uORB/topics/estimator_status.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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 estimator_status.h
+ * Definition of the estimator_status_report uORB topic.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#ifndef ESTIMATOR_STATUS_H_
+#define ESTIMATOR_STATUS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Estimator status report.
+ *
+ * This is a generic status report struct which allows any of the onboard estimators
+ * to write the internal state to the system log.
+ *
+ */
+struct estimator_status_report {
+
+	/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only   */
+
+	uint64_t timestamp;			/**< Timestamp in microseconds since boot */
+	float states[32];			/**< Internal filter states */
+	float n_states;				/**< Number of states effectively used */
+	uint8_t nan_flags;			/**< Bitmask to indicate NaN states */
+	uint8_t health_flags;			/**< Bitmask to indicate sensor health states (vel, pos, hgt) */
+	uint8_t timeout_flags;			/**< Bitmask to indicate timeout flags (vel, pos, hgt) */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(estimator_status);
+
+#endif
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.h b/src/modules/uORB/topics/fence.h
index c4c95f7b4..6f16c51cf 100755..100644
--- a/src/modules/position_estimator_mc/position_estimator_mc_params.h
+++ b/src/modules/uORB/topics/fence.h
@@ -1,9 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: 	Damian Aregger <daregger@student.ethz.ch>
- *   			Tobias Naegeli <naegelit@student.ethz.ch>
- *				Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,36 +32,49 @@
  *
  ****************************************************************************/
 
-/*
- * @file position_estimator_mc_params.h
- * 
- * Parameters for Position Estimator
+/**
+ * @file fence.h
+ * Definition of geofence.
  */
 
-#include <systemlib/param/param.h>
+#ifndef TOPIC_FENCE_H_
+#define TOPIC_FENCE_H_
 
-struct position_estimator_mc_params {
-	float addNoise;
-	float sigma;
-	float R;
-	int baro; /* consider barometer */
-};
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
 
-struct position_estimator_mc_param_handles {
-	param_t addNoise;
-	param_t sigma;
-	param_t r;
-	param_t baro_param_handle;
-};
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+#define GEOFENCE_MAX_VERTICES 16
 
 /**
- * Initialize all parameter handles and values
+ * This is the position of a geofence vertex
  *
  */
-int parameters_init(struct position_estimator_mc_param_handles *h);
+struct fence_vertex_s {
+	// Worst case float precision gives us 2 meter resolution at the equator
+	float lat;			/**< latitude in degrees */
+	float lon;			/**< longitude in degrees */
+};
 
 /**
- * Update all parameters
+ * This is the position of a geofence
  *
  */
-int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p);
+struct fence_s { 
+	unsigned count;     /**< number of actual vertices */
+	struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES];
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(fence);
+
+#endif
diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h
index ab6de2613..c5d545542 100644
--- a/src/modules/uORB/topics/filtered_bottom_flow.h
+++ b/src/modules/uORB/topics/filtered_bottom_flow.h
@@ -53,8 +53,7 @@
 /**
  * Filtered bottom flow in bodyframe.
  */
-struct filtered_bottom_flow_s
-{
+struct filtered_bottom_flow_s {
 	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
 
 	float sumx;				/**< Integrated bodyframe x flow in meters					   */
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 7e1b82a0f..70071130d 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.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
@@ -34,9 +35,10 @@
 
 /**
  * @file home_position.h
- * Definition of the GPS home position uORB topic.
+ * Definition of the home position uORB topic.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef TOPIC_HOME_POSITION_H_
@@ -55,16 +57,15 @@
  */
 struct home_position_s
 {
-	uint64_t timestamp;             /**< Timestamp (microseconds since system boot)   */
-	uint64_t time_gps_usec;         /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module   */
-	
-	int32_t lat;                    /**< Latitude in 1E7 degrees */
-	int32_t lon;                    /**< Longitude in 1E7 degrees */
-	int32_t alt;                    /**< Altitude in 1E3 meters (millimeters) above MSL */
-	float eph_m;                   /**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;                   /**< GPS VDOP horizontal dilution of position in m */
-	float s_variance_m_s;               /**< speed accuracy estimate m/s */
-	float p_variance_m;               /**< position accuracy estimate m */
+	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)	*/
+
+	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			*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index eac6d6e98..dde237adc 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -44,6 +44,16 @@
 #include "../uORB.h"
 
 /**
+ * Switch position
+ */
+typedef enum {
+	SWITCH_POS_NONE = 0,	/**< switch is not mapped */
+	SWITCH_POS_ON,			/**< switch activated (value = 1) */
+	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
+	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
+} switch_pos_t;
+
+/**
  * @addtogroup topics
  * @{
  */
@@ -51,32 +61,44 @@
 struct manual_control_setpoint_s {
 	uint64_t timestamp;
 
-	float roll;			 	/**< ailerons roll / roll rate input */
-	float pitch;				/**< elevator / pitch / pitch rate */
-	float yaw;				/**< rudder / yaw rate / yaw */
-	float throttle;				/**< throttle / collective thrust / altitude */
-
-	float mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
-	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
-	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
-	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
-
 	/**
-	 * Any of the channels below may not be available and be set to NaN
+	 * Any of the channels may not be available and be set to NaN
 	 * to indicate that it does not contain valid data.
+	 * The variable names follow the definition of the
+	 * MANUAL_CONTROL mavlink message.
+	 * The default range is from -1 to 1 (mavlink message -1000 to 1000)
+	 * The range for the z variable is defined from 0 to 1. (The z field of
+	 * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
 	 */
+	float x;			/**< stick position in x direction -1..1
+					  in general corresponds to forward/back motion or pitch of vehicle,
+					  in general a positive value means forward or negative pitch and
+					  a negative value means backward or positive pitch */
+	float y;			/**< stick position in y direction -1..1
+					  in general corresponds to right/left motion or roll of vehicle,
+					  in general a positive value means right or positive roll and
+					  a negative value means left or negative roll */
+	float z;			/**< throttle stick position 0..1
+					  in general corresponds to up/down motion or thrust of vehicle,
+					  in general the value corresponds to the demanded throttle by the user,
+					  if the input is used for setting the setpoint of a vertical position
+					  controller any value > 0.5 means up and any value < 0.5 means down */
+	float r;			/**< yaw stick/twist positon, -1..1
+					  in general corresponds to the righthand rotation around the vertical
+					  (downwards) axis of the vehicle */
+	float flaps;			/**< flap position */
+	float aux1;			/**< default function: camera yaw / azimuth */
+	float aux2;			/**< default function: camera pitch / tilt */
+	float aux3;			/**< default function: camera trigger */
+	float aux4;			/**< default function: camera roll */
+	float aux5;			/**< default function: payload drop */
 
-	// XXX needed or parameter?
-	//float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
-
-	float flaps;				/**< flap position */
-
-	float aux1;				/**< default function: camera yaw / azimuth */
-	float aux2;				/**< default function: camera pitch / tilt */
-	float aux3;				/**< default function: camera trigger */
-	float aux4;				/**< default function: camera roll */
-	float aux5;				/**< default function: payload drop */
-
+	switch_pos_t mode_switch;			/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+	switch_pos_t return_switch;			/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
+	switch_pos_t posctl_switch;			/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
+	switch_pos_t loiter_switch;			/**< loiter 2 position switch (optional): _MISSION_, LOITER */
+	switch_pos_t acro_switch;			/**< acro 2 position switch (optional): _MANUAL_, ACRO */
+	switch_pos_t offboard_switch;		/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
 }; /**< manual control inputs */
 
 /**
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 978a3383a..e4b72f87c 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
@@ -35,8 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file mission_item.h
- * Definition of one mission item.
+ * @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_
@@ -46,14 +46,26 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
+#define NUM_MISSIONS_SUPPORTED 256
+
+/* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
-	NAV_CMD_WAYPOINT = 0,
-	NAV_CMD_LOITER_TURN_COUNT,
-	NAV_CMD_LOITER_TIME_LIMIT,
-	NAV_CMD_LOITER_UNLIMITED,
-	NAV_CMD_RETURN_TO_LAUNCH,
-	NAV_CMD_LAND,
-	NAV_CMD_TAKEOFF
+	NAV_CMD_IDLE=0,
+	NAV_CMD_WAYPOINT=16,
+	NAV_CMD_LOITER_UNLIMITED=17,
+	NAV_CMD_LOITER_TURN_COUNT=18,
+	NAV_CMD_LOITER_TIME_LIMIT=19,
+	NAV_CMD_RETURN_TO_LAUNCH=20,
+	NAV_CMD_LAND=21,
+	NAV_CMD_TAKEOFF=22,
+	NAV_CMD_ROI=80,
+	NAV_CMD_PATHPLANNING=81,
+	NAV_CMD_DO_JUMP=177
+};
+
+enum ORIGIN {
+	ORIGIN_MAVLINK = 0,
+	ORIGIN_ONBOARD
 };
 
 /**
@@ -67,26 +79,34 @@ enum NAV_CMD {
  * This is the position the MAV is heading towards. If it of type loiter,
  * the MAV is circling around it with the given loiter radius in meters.
  */
-struct mission_item_s
-{
+struct mission_item_s {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	double lat;			/**< latitude in degrees * 1E7				*/
-	double lon;			/**< longitude in degrees * 1E7				*/
+	double lat;			/**< latitude in degrees				*/
+	double lon;			/**< longitude in degrees				*/
 	float altitude;			/**< altitude in meters					*/
-	float yaw;			/**< in radians NED -PI..+PI 				*/
+	float yaw;			/**< in radians NED -PI..+PI, NAN means don't change yaw		*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	uint8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
+	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
+	enum NAV_CMD nav_cmd;		/**< navigation command					*/
+	float acceptance_radius;	/**< default radius in which the mission is accepted as reached in meters */
+	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
+	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;	/**< 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	*/
 };
 
+/**
+ * This topic used to notify navigator about mission changes, mission itself and new mission state
+ * must be stored in dataman before publication.
+ */
 struct mission_s
 {
-	struct mission_item_s *items;
-	unsigned count;
+	int dataman_id;			/**< default 0, there are two offboard storage places in the dataman: 0 or 1 */
+	unsigned count;			/**< count of the missions stored in the dataman */
+	int current_seq;				/**< default -1, start at the one changed latest */
 };
 
 /**
@@ -94,6 +114,7 @@ struct mission_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(mission);
+ORB_DECLARE(offboard_mission);
+ORB_DECLARE(onboard_mission);
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/mission_result.h
index 8516b263f..beb797e62 100644
--- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -35,37 +35,28 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_position_setpoint.h
- * Definition of the global WGS84 position setpoint uORB topic.
+ * @file mission_result.h
+ * Mission results that navigator needs to pass on to commander and mavlink.
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
+#ifndef TOPIC_MISSION_RESULT_H
+#define TOPIC_MISSION_RESULT_H
 
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
 
-#include "vehicle_global_position_setpoint.h"
-
 /**
  * @addtogroup topics
  * @{
  */
 
-/**
- * Global position setpoint triplet in WGS84 coordinates.
- *
- * This are the three next waypoints (or just the next two or one).
- */
-struct vehicle_global_position_set_triplet_s
+struct mission_result_s
 {
-	bool previous_valid;					/**< flag indicating previous position is valid */
-	bool next_valid;					/**< flag indicating next position is valid */
-
-	struct vehicle_global_position_setpoint_s previous;
-	struct vehicle_global_position_setpoint_s current;
-	struct vehicle_global_position_setpoint_s next;
+	unsigned seq_reached;		/**< Sequence of the mission item which has been reached */
+	unsigned seq_current;		/**< Sequence of the current mission item				 */
+	bool reached;				/**< true if mission has been reached					 */
+	bool finished;				/**< true if mission has been completed					 */
 };
 
 /**
@@ -73,6 +64,6 @@ struct vehicle_global_position_set_triplet_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_set_triplet);
+ORB_DECLARE(mission_result);
 
 #endif
diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h
index 6a3e811e3..7a5ae9891 100644
--- a/src/modules/uORB/topics/navigation_capabilities.h
+++ b/src/modules/uORB/topics/navigation_capabilities.h
@@ -52,7 +52,12 @@
  * Airspeed
  */
 struct navigation_capabilities_s {
-    float turn_distance;    /**< the optimal distance to a waypoint to switch to the next */
+	float turn_distance;		/**< the optimal distance to a waypoint to switch to the next */
+
+	/* Landing parameters: see fw_pos_control_l1/landingslope.h */
+	float landing_horizontal_slope_displacement;
+	float landing_slope_angle_rad;
+	float landing_flare_length;
 };
 
 /**
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 7901b930a..d7b131e3c 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -45,12 +45,11 @@
 
 /**
  * Off-board control inputs.
- * 
+ *
  * Typically sent by a ground control station / joystick or by
  * some off-board controller via C or SIMULINK.
  */
-enum OFFBOARD_CONTROL_MODE
-{
+enum OFFBOARD_CONTROL_MODE {
 	OFFBOARD_CONTROL_MODE_DIRECT = 0,
 	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
 	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
@@ -70,7 +69,7 @@ struct offboard_control_setpoint_s {
 	uint64_t timestamp;
 
 	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
-	bool armed;	/**< Armed flag set, yes / no */
+
 	float p1;	/**< ailerons roll / roll rate input */
 	float p2;	/**< elevator / pitch / pitch rate */
 	float p3;	/**< rudder / yaw rate / yaw */
diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index 98f0e3fa2..0196ae86b 100644
--- a/src/modules/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
@@ -57,6 +57,7 @@ struct optical_flow_s {
 
 	uint64_t timestamp;		/**< in microseconds since system start          */
 
+	uint64_t flow_timestamp;		/**< timestamp from flow sensor */
 	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
 	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
 	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
new file mode 100644
index 000000000..673c0e491
--- /dev/null
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   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 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_
+#define TOPIC_MISSION_ITEM_TRIPLET_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+enum SETPOINT_TYPE
+{
+	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) */
+	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint set by offboard */
+};
+
+struct position_setpoint_s
+{
+	bool valid;			/**< true if setpoint is valid */
+	enum SETPOINT_TYPE type;	/**< setpoint type to adjust behavior of position controller */
+	float x;			/**< local position setpoint in m in NED */
+	float y;			/**< local position setpoint in m in NED */
+	float z;			/**< local position setpoint in m in NED */
+	bool position_valid;	/**< true if local position setpoint valid */
+	float vx;			/**< local velocity setpoint in m in NED */
+	float vy;			/**< local velocity setpoint in m in NED */
+	float vz;			/**< local velocity setpoint in m in NED */
+	bool velocity_valid;		/**< true if local velocity setpoint valid */
+	double lat;			/**< latitude, in deg */
+	double lon;			/**< longitude, in deg */
+	float alt;			/**< altitude AMSL, in m */
+	float yaw;			/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
+	float yawspeed;			/**< yawspeed (only for multirotors, in rad/s) */
+	float loiter_radius;		/**< loiter radius (only for fixed wing), in m */
+	int8_t loiter_direction;	/**< loiter direction: 1 = CW, -1 = CCW */
+	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
+};
+
+/**
+ * Global position setpoint triplet in WGS84 coordinates.
+ *
+ * This are the three next waypoints (or just the next two or one).
+ */
+struct position_setpoint_triplet_s
+{
+	struct position_setpoint_s previous;
+	struct position_setpoint_s current;
+	struct position_setpoint_s next;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(position_setpoint_triplet);
+
+#endif
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 086b2ef15..8978de471 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -43,58 +43,43 @@
 #include "../uORB.h"
 
 /**
- * The number of RC channel inputs supported.
- * Current (Q4/2013) radios support up to 18 channels,
- * leaving at a sane value of 15.
- * This number can be greater then number of RC channels,
- * because single RC channel can be mapped to multiple
- * functions, e.g. for various mode switches.
- */
-#define RC_CHANNELS_MAPPED_MAX   15
-
-/** 
  * This defines the mapping of the RC functions.
  * The value assigned to the specific function corresponds to the entry of
- * the channel array chan[].
+ * the channel array channels[].
  */
-enum RC_CHANNELS_FUNCTION
-{
-  THROTTLE = 0,
-  ROLL     = 1,
-  PITCH    = 2,
-  YAW      = 3,
-  MODE = 4,
-  RETURN = 5,
-  ASSISTED = 6,
-  MISSION = 7,
-  OFFBOARD_MODE = 8,
-  FLAPS   = 9,
-  AUX_1   = 10,
-  AUX_2   = 11,
-  AUX_3   = 12,
-  AUX_4   = 13,
-  AUX_5   = 14,
-  RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
+enum RC_CHANNELS_FUNCTION {
+	THROTTLE = 0,
+	ROLL,
+	PITCH,
+	YAW,
+	MODE,
+	RETURN,
+	POSCTL,
+	LOITER,
+	OFFBOARD,
+	ACRO,
+	FLAPS,
+	AUX_1,
+	AUX_2,
+	AUX_3,
+	AUX_4,
+	AUX_5,
+	RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
 };
 
 /**
  * @addtogroup topics
  * @{
  */
-
 struct rc_channels_s {
-
-	uint64_t timestamp;                 /**< In microseconds since boot time. */
-  uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
-  struct {
-    float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-  } chan[RC_CHANNELS_MAPPED_MAX];
-  uint8_t chan_count;                 /**< number of valid channels */
-
-  /*String array to store the names of the functions*/
-  char function_name[RC_CHANNELS_FUNCTION_MAX][20];
-  int8_t function[RC_CHANNELS_FUNCTION_MAX];
-  uint8_t rssi;                       /**< Overall receive signal strength */
+	uint64_t timestamp;									/**< Timestamp in microseconds since boot time */
+	uint64_t timestamp_last_valid;						/**< Timestamp of last valid RC signal */
+	float channels[RC_CHANNELS_FUNCTION_MAX];			/**< Scaled to -1..1 (throttle: 0..1) */
+	uint8_t channel_count;								/**< Number of valid channels */
+	char function_name[RC_CHANNELS_FUNCTION_MAX][20];	/**< String array to store the names of the functions */
+	int8_t function[RC_CHANNELS_FUNCTION_MAX];			/**< Functions mapping */
+	uint8_t rssi;										/**< Receive signal strength index */
+	bool signal_lost;									/**< Control signal lost, should be checked together with topic timeout */
 }; /**< radio control channels. */
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/satellite_info.h
index a56434d3b..37c2faa96 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/satellite_info.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2014 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>
@@ -35,17 +35,15 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_position_setpoint.h
- * Definition of the global WGS84 position setpoint uORB topic.
+ * @file satellite_info.h
+ * Definition of the GNSS satellite info uORB topic.
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
+#ifndef TOPIC_SAT_INFO_H_
+#define TOPIC_SAT_INFO_H_
 
 #include <stdint.h>
-#include <stdbool.h>
 #include "../uORB.h"
-#include "mission.h"
 
 /**
  * @addtogroup topics
@@ -53,34 +51,39 @@
  */
 
 /**
- * Global position setpoint in WGS84 coordinates.
- *
- * This is the position the MAV is heading towards. If it of type loiter,
- * the MAV is circling around it with the given loiter radius in meters.
+ * GNSS Satellite Info.
  */
-struct vehicle_global_position_setpoint_s
-{
-	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	int32_t lat;			/**< latitude in degrees * 1E7				*/
-	int32_t lon;			/**< longitude in degrees * 1E7				*/
-	float altitude;			/**< altitude in meters					*/
-	float yaw;			/**< in radians NED -PI..+PI 				*/
-	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
-	float turn_distance_xy;		/**< The distance on the plane which will mark this as reached */
-	float turn_distance_z;		/**< The distance in Z direction which will mark this as reached */
+
+#define SAT_INFO_MAX_SATELLITES  20
+
+struct satellite_info_s {
+	uint64_t timestamp;				/**< Timestamp of satellite info */
+	uint8_t count;					/**< Number of satellites in satellite info */
+	uint8_t svid[SAT_INFO_MAX_SATELLITES]; 		/**< Space vehicle ID [1..255], see scheme below  */
+	uint8_t used[SAT_INFO_MAX_SATELLITES];		/**< 0: Satellite not used, 1: used for navigation */
+	uint8_t elevation[SAT_INFO_MAX_SATELLITES];	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+	uint8_t azimuth[SAT_INFO_MAX_SATELLITES];	/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t snr[SAT_INFO_MAX_SATELLITES];		/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
 };
 
 /**
+ * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs
+ * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf
+ *
+ * GPS		1-32
+ * SBAS		120-158
+ * Galileo	211-246
+ * BeiDou	159-163, 33-64
+ * QZSS		193-197
+ * GLONASS	65-96, 255
+ *
+ */
+
+/**
  * @}
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_setpoint);
+ORB_DECLARE(satellite_info);
 
 #endif
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index ad164555e..fa3367de9 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -77,34 +77,36 @@ struct sensor_combined_s {
 
 	/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only   */
 
-	uint64_t timestamp;			/**< Timestamp in microseconds since boot         */
+	uint64_t timestamp;			/**< Timestamp in microseconds since boot, from gyro         */
 
 	int16_t	gyro_raw[3];			/**< Raw sensor values of angular velocity        */
-	uint16_t gyro_counter;			/**< Number of raw measurments taken              */
 	float gyro_rad_s[3];			/**< Angular velocity in radian per seconds       */
-	
+
 	int16_t accelerometer_raw[3];		/**< Raw acceleration in NED body frame           */
-	uint32_t accelerometer_counter;		/**< Number of raw acc measurements taken         */
 	float accelerometer_m_s2[3];		/**< Acceleration in NED body frame, in m/s^2     */
 	int accelerometer_mode;			/**< Accelerometer measurement mode */
 	float accelerometer_range_m_s2;		/**< Accelerometer measurement range in m/s^2 */
+	uint64_t accelerometer_timestamp;	/**< Accelerometer timestamp        */
 
 	int16_t	magnetometer_raw[3];		/**< Raw magnetic field in NED body frame         */
 	float magnetometer_ga[3];		/**< Magnetic field in NED body frame, in Gauss   */
 	int magnetometer_mode;			/**< Magnetometer measurement mode */
 	float magnetometer_range_ga;		/**< ± measurement range in Gauss */
 	float magnetometer_cuttoff_freq_hz;	/**< Internal analog low pass frequency of sensor */
-	uint32_t magnetometer_counter;		/**< Number of raw mag measurements taken         */
-	
+	uint64_t magnetometer_timestamp;	/**< Magnetometer timestamp         */
+
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */
-	float adc_voltage_v[4];			/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
+	float adc_voltage_v[10];		/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
+	unsigned adc_mapping[10];		/**< Channel indices of each of these values */
 	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
-	uint32_t baro_counter;			/**< Number of raw baro measurements taken        */
+	uint64_t baro_timestamp;		/**< Barometer timestamp        */
+
+	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */
+	uint64_t differential_pressure_timestamp;		/**< Last measurement timestamp */
+	float differential_pressure_filtered_pa;	/**< Low pass filtered airspeed sensor differential pressure reading */
 
-	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */ 
-	uint32_t differential_pressure_counter;		/**< Number of raw differential pressure measurements taken */
 };
 
 /**
diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h
index cfe0bf69e..d3a7ce10d 100644
--- a/src/modules/uORB/topics/subsystem_info.h
+++ b/src/modules/uORB/topics/subsystem_info.h
@@ -50,8 +50,7 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-enum SUBSYSTEM_TYPE
-{
+enum SUBSYSTEM_TYPE {
 	SUBSYSTEM_TYPE_GYRO = 1,
 	SUBSYSTEM_TYPE_ACC = 2,
 	SUBSYSTEM_TYPE_MAG = 4,
diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/modules/uORB/topics/system_power.h
index dcb85600e..7763b6004 100644
--- a/src/lib/mathlib/math/Vector3.cpp
+++ b/src/modules/uORB/topics/system_power.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-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
@@ -32,68 +32,40 @@
  ****************************************************************************/
 
 /**
- * @file Vector3.cpp
+ * @file system_power.h
  *
- * math vector
+ * Definition of the system_power voltage and power status uORB topic.
  */
 
-#include "test/test.hpp"
+#ifndef SYSTEM_POWER_H_
+#define SYSTEM_POWER_H_
 
-#include "Vector3.hpp"
+#include "../uORB.h"
+#include <stdint.h>
 
-namespace math
-{
-
-Vector3::Vector3() :
-	Vector(3)
-{
-}
-
-Vector3::Vector3(const Vector &right) :
-	Vector(right)
-{
-#ifdef VECTOR_ASSERT
-	ASSERT(right.getRows() == 3);
-#endif
-}
-
-Vector3::Vector3(float x, float y, float z) :
-	Vector(3)
-{
-	setX(x);
-	setY(y);
-	setZ(z);
-}
-
-Vector3::Vector3(const float *data) :
-	Vector(3, data)
-{
-}
+/**
+ * @addtogroup topics
+ * @{
+ */
 
-Vector3::~Vector3()
-{
-}
+/**
+ * voltage and power supply status
+ */
+struct system_power_s {
+	uint64_t	timestamp;		/**< microseconds since system boot */
+	float   	voltage5V_v;		/**< peripheral 5V rail voltage */
+	uint8_t         usb_connected:1;        /**< USB is connected when 1 */
+	uint8_t         brick_valid:1;          /**< brick power is good when 1 */
+	uint8_t         servo_valid:1;          /**< servo power is good when 1 */
+	uint8_t		periph_5V_OC:1;         /**< peripheral overcurrent when 1 */
+	uint8_t		hipower_5V_OC:1;        /**< hi power peripheral overcurrent when 1 */
+};
 
-Vector3 Vector3::cross(const Vector3 &b) const
-{
-	const Vector3 &a = *this;
-	Vector3 result;
-	result(0) = a(1) * b(2) - a(2) * b(1);
-	result(1) = a(2) * b(0) - a(0) * b(2);
-	result(2) = a(0) * b(1) - a(1) * b(0);
-	return result;
-}
+/**
+ * @}
+ */
 
-int __EXPORT vector3Test()
-{
-	printf("Test Vector3\t\t: ");
-	// test float ctor
-	Vector3 v(1, 2, 3);
-	ASSERT(equal(v(0), 1));
-	ASSERT(equal(v(1), 2));
-	ASSERT(equal(v(2), 3));
-	printf("PASS\n");
-	return 0;
-}
+/* register this as object request broker structure */
+ORB_DECLARE(system_power);
 
-} // namespace math
+#endif
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/uORB/topics/tecs_status.h
index ca7794c59..c4d0c1874 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -1,12 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Lorenz Meier <lm@inf.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
@@ -37,28 +31,63 @@
  *
  ****************************************************************************/
 
-/*
- * @file multirotor_attitude_control.h
- *
- * Definition of rate controller for multirotors.
+/**
+ * @file vehicle_global_position.h
+ * Definition of the global fused WGS84 position uORB topic.
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#ifndef MULTIROTOR_RATE_CONTROL_H_
-#define MULTIROTOR_RATE_CONTROL_H_
+#ifndef TECS_STATUS_T_H_
+#define TECS_STATUS_T_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+typedef enum {
+	TECS_MODE_NORMAL,
+	TECS_MODE_UNDERSPEED,
+	TECS_MODE_TAKEOFF,
+	TECS_MODE_LAND,
+	TECS_MODE_LAND_THROTTLELIM
+} tecs_mode;
 
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
+ /**
+ * Internal values of the (m)TECS fixed wing speed alnd altitude control system
+ */
+struct tecs_status_s {
+	uint64_t timestamp;		/**< timestamp, in microseconds since system start */
+
+	float altitudeSp;
+	float altitude;
+	float flightPathAngleSp;
+	float flightPathAngle;
+	float flightPathAngleFiltered;
+	float airspeedSp;
+	float airspeed;
+	float airspeedFiltered;
+	float airspeedDerivativeSp;
+	float airspeedDerivative;
+
+	float totalEnergyRateSp;
+	float totalEnergyRate;
+	float energyDistributionRateSp;
+	float energyDistributionRate;
+
+	tecs_mode mode;
+};
+
+/**
+ * @}
+ */
 
-void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
+/* register this as object request broker structure */
+ORB_DECLARE(tecs_status);
 
-#endif /* MULTIROTOR_RATE_CONTROL_H_ */
+#endif
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index 828fb31cc..3347724a5 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -44,10 +44,10 @@
 #include "../uORB.h"
 
 enum TELEMETRY_STATUS_RADIO_TYPE {
-    TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0,
-    TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO,
-    TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET,
-    TELEMETRY_STATUS_RADIO_TYPE_WIRE
+	TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0,
+	TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO,
+	TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET,
+	TELEMETRY_STATUS_RADIO_TYPE_WIRE
 };
 
 /**
@@ -57,20 +57,43 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 
 struct telemetry_status_s {
 	uint64_t timestamp;
-    enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
-	unsigned rssi;              /**< local signal strength                      */
-    unsigned remote_rssi;       /**< remote signal strength                     */
-    unsigned rxerrors;          /**< receive errors                             */
-    unsigned fixed;             /**< count of error corrected packets           */
-    uint8_t noise;              /**< background noise level                     */
-    uint8_t remote_noise;       /**< remote background noise level              */
-    uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
+	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                     */
+	uint16_t rxerrors;			/**< receive errors                             */
+	uint16_t fixed;				/**< count of error corrected packets           */
+	uint8_t noise;				/**< background noise level                     */
+	uint8_t remote_noise;			/**< remote background noise level              */
+	uint8_t txbuf;				/**< how full the tx buffer is as a percentage  */
 };
 
 /**
  * @}
  */
 
-ORB_DECLARE(telemetry_status);
+ORB_DECLARE(telemetry_status_0);
+ORB_DECLARE(telemetry_status_1);
+ORB_DECLARE(telemetry_status_2);
+ORB_DECLARE(telemetry_status_3);
 
-#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file
+#define TELEMETRY_STATUS_ORB_ID_NUM	4
+
+static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = {
+	ORB_ID(telemetry_status_0),
+	ORB_ID(telemetry_status_1),
+	ORB_ID(telemetry_status_2),
+	ORB_ID(telemetry_status_3),
+};
+
+// This is a hack to quiet an unused-variable warning for when telemetry_status.h is
+// included but telemetry_status_orb_id is not referenced. The inline works if you
+// choose to use it, but you can continue to just directly index into the array as well.
+// If you don't use the inline this ends up being a no-op with no additional code emitted.
+extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index);
+extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index)
+{
+	return telemetry_status_orb_id[index];
+}
+
+#endif /* TOPIC_TELEMETRY_STATUS_H */
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index 4380a5ee7..40328af14 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -76,6 +76,7 @@ struct vehicle_attitude_s {
 	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
 	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
 	float q[4];		/**< Quaternion (NED)						*/
+	float g_comp[3];	/**< Compensated gravity vector					*/
 	bool R_valid;		/**< Rotation matrix valid					*/
 	bool q_valid;		/**< Quaternion valid						*/
 
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index 1a245132a..8446e9c6e 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -52,8 +52,7 @@
 /**
  * vehicle attitude setpoint.
  */
-struct vehicle_attitude_setpoint_s
-{
+struct vehicle_attitude_setpoint_s {
 	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	float roll_body;				/**< body angle in NED frame		*/
@@ -61,7 +60,7 @@ struct vehicle_attitude_setpoint_s
 	float yaw_body;					/**< body angle in NED frame		*/
 	//float body_valid;				/**< Set to true if body angles are valid */
 
-	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
+	float R_body[3][3];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
 	bool R_valid;					/**< Set to true if rotation matrix is valid */
 
 	//! For quaternion-based attitude control
@@ -73,6 +72,8 @@ struct vehicle_attitude_setpoint_s
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
 	bool	roll_reset_integral;			/**< Reset roll integral part (navigation logic change) */
+	bool	pitch_reset_integral;			/**< Reset pitch integral part (navigation logic change) */
+	bool	yaw_reset_integral;			/**< Reset yaw integral part (navigation logic change) */
 
 };
 
diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h
index fbfab09f3..fd1ade671 100644
--- a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h
@@ -48,8 +48,7 @@
  * @{
  */
 
-struct vehicle_bodyframe_speed_setpoint_s
-{
+struct vehicle_bodyframe_speed_setpoint_s {
 	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	float vx;		/**< in m/s				  		*/
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index e6e4743c6..c21a29b13 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -51,43 +51,42 @@
  * Should contain all commands from MAVLink's VEHICLE_CMD ENUM,
  * but can contain additional ones.
  */
-enum VEHICLE_CMD
-{
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_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|  */
-	VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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|  */
-	VEHICLE_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)|  */
-	VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
-	VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
-	VEHICLE_CMD_ENUM_END=501, /*  | */
+enum VEHICLE_CMD {
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_NAV_LAND = 21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_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|  */
+	VEHICLE_CMD_CONDITION_DISTANCE = 114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_CMD_DO_SET_MODE = 176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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|  */
+	VEHICLE_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)|  */
+	VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
+	VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
+	VEHICLE_CMD_ENUM_END = 501, /*  | */
 };
 
 /**
@@ -96,14 +95,13 @@ enum VEHICLE_CMD
  * Should contain all of MAVLink's VEHICLE_CMD_RESULT values
  * but can contain additional ones.
  */
-enum VEHICLE_CMD_RESULT
-{
-	VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */
-	VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */
-	VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */
-	VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */
-	VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */
-	VEHICLE_CMD_RESULT_ENUM_END=5, /*  | */
+enum VEHICLE_CMD_RESULT {
+	VEHICLE_CMD_RESULT_ACCEPTED = 0, /* Command ACCEPTED and EXECUTED | */
+	VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1, /* Command TEMPORARY REJECTED/DENIED | */
+	VEHICLE_CMD_RESULT_DENIED = 2, /* Command PERMANENTLY DENIED | */
+	VEHICLE_CMD_RESULT_UNSUPPORTED = 3, /* Command UNKNOWN/UNSUPPORTED | */
+	VEHICLE_CMD_RESULT_FAILED = 4, /* Command executed, but failed | */
+	VEHICLE_CMD_RESULT_ENUM_END = 5, /*  | */
 };
 
 /**
@@ -111,8 +109,7 @@ enum VEHICLE_CMD_RESULT
  * @{
  */
 
-struct vehicle_command_s
-{
+struct vehicle_command_s {
 	float param1;			/**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param2;			/**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param3;			/**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum.   */
diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h
index 6184284a4..2a45eaccc 100644
--- a/src/modules/uORB/topics/vehicle_control_debug.h
+++ b/src/modules/uORB/topics/vehicle_control_debug.h
@@ -47,8 +47,7 @@
  * @addtogroup topics
  * @{
  */
-struct vehicle_control_debug_s
-{
+struct vehicle_control_debug_s {
 	uint64_t timestamp; /**< in microseconds since system start */
 
 	float roll_p;		/**< roll P control part		*/
@@ -77,9 +76,9 @@ struct vehicle_control_debug_s
 
 }; /**< vehicle_control_debug */
 
- /**
- * @}
- */
+/**
+* @}
+*/
 
 /* register this as object request broker structure */
 ORB_DECLARE(vehicle_control_debug);
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 38fb74c9b..49e2ba4b5 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.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
@@ -38,8 +34,13 @@
 /**
  * @file vehicle_control_mode.h
  * Definition of the vehicle_control_mode uORB topic.
- * 
+ *
  * All control apps should depend their actions based on the flags set here.
+ *
+ * @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>
  */
 
 #ifndef VEHICLE_CONTROL_MODE
@@ -48,6 +49,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
+#include "vehicle_status.h"
 
 /**
  * @addtogroup topics @{
@@ -59,8 +61,8 @@
  *
  * Encodes the complete system state and is set by the commander app.
  */
-struct vehicle_control_mode_s
-{
+
+struct vehicle_control_mode_s {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	bool flag_armed;
@@ -71,16 +73,15 @@ struct vehicle_control_mode_s
 	bool flag_system_hil_enabled;
 
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
+	bool flag_control_auto_enabled;			/**< true if onboard autopilot should act */
+	bool flag_control_offboard_enabled;		/**< true if offboard control should be used */
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
 	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
-	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
-
-	bool flag_control_auto_enabled;		// TEMP
-	uint8_t auto_state;	// TEMP navigation state for AUTO modes
+	bool flag_control_climb_rate_enabled;	/**< true if climb rate is controlled */
+	bool flag_control_termination_enabled;	/**< true if flighttermination is enabled */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h
new file mode 100644
index 000000000..e3a7360b2
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_force_setpoint.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 vehicle_force_setpoint.h
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * Definition of force (NED) setpoint uORB topic. Typically this can be used
+ * by a position control app together with an attitude control app.
+ */
+
+#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_
+#define TOPIC_VEHICLE_FORCE_SETPOINT_H_
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_force_setpoint_s {
+	float x;		/**< in N NED			  		*/
+	float y;		/**< in N NED			  		*/
+	float z;		/**< in N NED			  		*/
+	float yaw;		/**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */
+}; /**< Desired force in NED frame */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_force_setpoint);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 143734e37..e32529cb4 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.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>
+ *   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
@@ -37,6 +34,10 @@
 /**
  * @file vehicle_global_position.h
  * Definition of the global fused WGS84 position uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef VEHICLE_GLOBAL_POSITION_T_H_
@@ -54,25 +55,23 @@
  /**
  * Fused global position in WGS84.
  *
- * This struct contains the system's believ about its position. It is not the raw GPS
+ * This struct contains global position estimation. It is not the raw GPS
  * measurement (@see vehicle_gps_position). This topic is usually published by the position
  * estimator, which will take more sources of information into account than just GPS,
  * e.g. control inputs of the vehicle in a Kalman-filter implementation.
  */
-struct vehicle_global_position_s
-{
-	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
-	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds							   */
-	bool valid;			/**< true if position satisfies validity criteria of estimator */
-
-	int32_t lat;			/**< Latitude in 1E7 degrees							 	   */
-	int32_t lon;			/**< Longitude in 1E7 degrees							 	   */
-	float alt;			/**< Altitude in meters									 	   */
-	float relative_alt;		/**< Altitude above home position in meters, 				   */
-	float vx; 			/**< Ground X velocity, m/s in NED				 			   */
-	float vy;			/**< Ground Y velocity, m/s in NED							   */
-	float vz;			/**< Ground Z velocity, m/s	in NED 							   */
-	float yaw; 			/**< Compass heading in radians -PI..+PI.					   */
+struct vehicle_global_position_s {
+	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start		*/
+	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 yaw; 			/**< Yaw in radians -PI..+PI.							   */
+	float eph;
+	float epv;
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
index 73961cdfe..5dac877d0 100644
--- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
@@ -47,8 +47,7 @@
  * @{
  */
 
-struct vehicle_global_velocity_setpoint_s
-{
+struct vehicle_global_velocity_setpoint_s {
 	float vx;		/**< in m/s NED			  		*/
 	float vy;		/**< in m/s NED			  		*/
 	float vz;		/**< in m/s NED			  		*/
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 1639a08c2..80d65cd69 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -53,21 +53,22 @@
 /**
  * GPS position in WGS84 coordinates.
  */
-struct vehicle_gps_position_s
-{
+struct vehicle_gps_position_s {
 	uint64_t timestamp_position;			/**< Timestamp for position information */
 	int32_t lat;					/**< Latitude in 1E-7 degrees */
 	int32_t lon;					/**< Longitude in 1E-7 degrees */
 	int32_t alt;					/**< Altitude in 1E-3 meters (millimeters) above MSL  */
-	
+
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
-	float p_variance_m;				/**< position accuracy estimate m */
 	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 */
+
+	unsigned noise_per_ms;				/**< */
+	unsigned jamming_indicator;			/**< */
 
 	uint64_t timestamp_velocity;			/**< Timestamp for velocity informations */
 	float vel_m_s;					/**< GPS ground speed (m/s) */
@@ -80,14 +81,7 @@ struct vehicle_gps_position_s
 	uint64_t timestamp_time;			/**< Timestamp for time information */
 	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
 
-	uint64_t timestamp_satellites;			/**< Timestamp for sattelite information */
-	uint8_t satellites_visible;			/**< Number of satellites visible. If unknown, set to 255 */
-	uint8_t satellite_prn[20]; 			/**< Global satellite ID */
-	uint8_t satellite_used[20];			/**< 0: Satellite not used, 1: used for localization */
-	uint8_t satellite_elevation[20]; 		/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
-	uint8_t satellite_azimuth[20];			/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
-	uint8_t satellite_snr[20];			/**< Signal to noise ratio of satellite   */
-	bool satellite_info_available;			/**< 0 for no info, 1 for info available */
+	uint8_t satellites_used;			/**< Number of satellites used */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 427153782..5d39c897d 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @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
@@ -35,6 +34,9 @@
 /**
  * @file vehicle_local_position.h
  * Definition of the local fused NED position uORB topic.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_
@@ -52,8 +54,7 @@
 /**
  * Fused local position in NED.
  */
-struct vehicle_local_position_s
-{
+struct vehicle_local_position_s {
 	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
 	bool xy_valid;			/**< true if x and y are valid */
 	bool z_valid;			/**< true if z is valid */
@@ -73,10 +74,17 @@ struct vehicle_local_position_s
 	bool xy_global;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
 	bool z_global;			/**< true if z is valid and has valid global reference (ref_alt) */
 	uint64_t ref_timestamp;	/**< Time when reference position was set */
-	int32_t ref_lat;		/**< Reference point latitude in 1E7 degrees */
-	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
+	double ref_lat;		/**< Reference point latitude in degrees */
+	double ref_lon;		/**< Reference point longitude in degrees */
 	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
 	bool landed;			/**< true if vehicle is landed */
+	/* Distance to surface */
+	float dist_bottom;		/**< Distance to bottom surface (ground) */
+	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */
+	uint64_t surface_bottom_timestamp;		/**< Time when new bottom surface found */
+	bool dist_bottom_valid;	/**< true if distance to bottom surface is valid */
+	float eph;
+	float epv;
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
index d24d81e3a..8988a0330 100644
--- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
@@ -49,8 +49,7 @@
  * @{
  */
 
-struct vehicle_local_position_setpoint_s
-{
+struct vehicle_local_position_setpoint_s {
 	float x;		/**< in meters NED			  		*/
 	float y;		/**< in meters NED			  		*/
 	float z;		/**< in meters NED			  		*/
diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h
index 46e62c4b7..9f8b412a7 100644
--- a/src/modules/uORB/topics/vehicle_rates_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h
@@ -47,8 +47,7 @@
  * @addtogroup topics
  * @{
  */
-struct vehicle_rates_setpoint_s
-{
+struct vehicle_rates_setpoint_s {
 	uint64_t timestamp; /**< in microseconds since system start */
 
 	float roll;	/**< body angular rates in NED frame		*/
@@ -58,9 +57,9 @@ struct vehicle_rates_setpoint_s
 
 }; /**< vehicle_rates_setpoint */
 
- /**
- * @}
- */
+/**
+* @}
+*/
 
 /* register this as object request broker structure */
 ORB_DECLARE(vehicle_rates_setpoint);
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 6ea48a680..b46c00b75 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_
@@ -58,28 +59,23 @@
  * @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_SEATBELT,
-	MAIN_STATE_EASY,
-	MAIN_STATE_AUTO,
+	MAIN_STATE_ALTCTL,
+	MAIN_STATE_POSCTL,
+	MAIN_STATE_AUTO_MISSION,
+	MAIN_STATE_AUTO_LOITER,
+	MAIN_STATE_AUTO_RTL,
+	MAIN_STATE_ACRO,
+	MAIN_STATE_OFFBOARD,
+	MAIN_STATE_MAX
 } main_state_t;
 
-/* navigation state machine */
-typedef enum {
-	NAVIGATION_STATE_DIRECT = 0,		// true manual control, no any stabilization
-	NAVIGATION_STATE_STABILIZE,		// attitude stabilization
-	NAVIGATION_STATE_ALTHOLD,		// attitude + altitude stabilization
-	NAVIGATION_STATE_VECTOR,		// attitude + altitude + position stabilization
-	NAVIGATION_STATE_AUTO_READY,	// AUTO, landed, reeady for takeoff
-	NAVIGATION_STATE_AUTO_TAKEOFF,	// detect takeoff using land detector and switch to desired AUTO mode
-	NAVIGATION_STATE_AUTO_LOITER,	// pause mission
-	NAVIGATION_STATE_AUTO_MISSION,	// fly mission
-	NAVIGATION_STATE_AUTO_RTL,		// Return To Launch, when home position switch to LAND
-	NAVIGATION_STATE_AUTO_LAND		// land and switch to AUTO_READY when landed (detect using land detector)
-} navigation_state_t;
-
+// If you change the order, add or remove arming_state_t states make sure to update the arrays
+// in state_machine_helper.cpp as well.
 typedef enum {
 	ARMING_STATE_INIT = 0,
 	ARMING_STATE_STANDBY,
@@ -87,7 +83,8 @@ typedef enum {
 	ARMING_STATE_ARMED_ERROR,
 	ARMING_STATE_STANDBY_ERROR,
 	ARMING_STATE_REBOOT,
-	ARMING_STATE_IN_AIR_RESTORE
+	ARMING_STATE_IN_AIR_RESTORE,
+	ARMING_STATE_MAX,
 } arming_state_t;
 
 typedef enum {
@@ -95,26 +92,24 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
+/**
+ * Navigation state, i.e. "what should vehicle do".
+ */
 typedef enum {
-	MODE_SWITCH_MANUAL = 0,
-	MODE_SWITCH_ASSISTED,
-	MODE_SWITCH_AUTO
-} mode_switch_pos_t;
-
-typedef enum {
-	ASSISTED_SWITCH_SEATBELT = 0,
-	ASSISTED_SWITCH_EASY
-} assisted_switch_pos_t;
-
-typedef enum {
-	RETURN_SWITCH_NONE = 0,
-	RETURN_SWITCH_RETURN
-} return_switch_pos_t;
-
-typedef enum {
-	MISSION_SWITCH_NONE = 0,
-	MISSION_SWITCH_MISSION
-} mission_switch_pos_t;
+	NAVIGATION_STATE_MANUAL = 0,		/**< Manual 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 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) */
+	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
+	NAVIGATION_STATE_OFFBOARD,
+	NAVIGATION_STATE_MAX,
+} navigation_state_t;
 
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -131,31 +126,31 @@ enum VEHICLE_MODE_FLAG {
  * Should match 1:1 MAVLink's MAV_TYPE ENUM
  */
 enum VEHICLE_TYPE {
-	VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */
-	VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */
-	VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */
-	VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */
-	VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */
-	VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */
-	VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */
-	VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */
-	VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */
-	VEHICLE_TYPE_ROCKET=9, /* Rocket | */
-	VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */
-	VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */
-	VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */
-	VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */
-	VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */
-	VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */
-	VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */
-	VEHICLE_TYPE_KITE=17, /* Kite | */
-	VEHICLE_TYPE_ENUM_END=18, /*  | */
+	VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */
+	VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */
+	VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */
+	VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */
+	VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */
+	VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */
+	VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */
+	VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */
+	VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */
+	VEHICLE_TYPE_ROCKET = 9, /* Rocket | */
+	VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */
+	VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */
+	VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */
+	VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */
+	VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */
+	VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
+	VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
+	VEHICLE_TYPE_KITE = 17, /* Kite | */
+	VEHICLE_TYPE_ENUM_END = 18, /*  | */
 };
 
 enum VEHICLE_BATTERY_WARNING {
-    VEHICLE_BATTERY_WARNING_NONE = 0,	/**< no battery low voltage warning active */
-    VEHICLE_BATTERY_WARNING_LOW,	/**< warning of low voltage */
-    VEHICLE_BATTERY_WARNING_CRITICAL	/**< alerting of critical voltage */
+	VEHICLE_BATTERY_WARNING_NONE = 0,	/**< no battery low voltage warning active */
+	VEHICLE_BATTERY_WARNING_LOW,	/**< warning of low voltage */
+	VEHICLE_BATTERY_WARNING_CRITICAL	/**< alerting of critical voltage */
 };
 
 /**
@@ -168,17 +163,17 @@ enum VEHICLE_BATTERY_WARNING {
  *
  * Encodes the complete system state and is set by the commander app.
  */
-struct vehicle_status_s
-{
+struct vehicle_status_s {
 	/* use of a counter and timestamp recommended (but not necessary) */
 
 	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 */
-	navigation_state_t navigation_state;	/**< navigation state machine */
+	main_state_t main_state;		    	/**< main state machine */
+	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 */
+	hil_state_t hil_state;				/**< current hil 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 */
@@ -186,11 +181,6 @@ struct vehicle_status_s
 
 	bool is_rotary_wing;
 
-	mode_switch_pos_t mode_switch;
-	return_switch_pos_t return_switch;
-	assisted_switch_pos_t assisted_switch;
-	mission_switch_pos_t mission_switch;
-
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
 	bool condition_system_sensors_initialized;
@@ -203,11 +193,15 @@ struct vehicle_status_s
 	bool condition_local_altitude_valid;
 	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
+	bool condition_power_input_valid;		/**< set if input power is valid */
+	float avionics_power_rail_voltage;		/**< voltage of the avionics power rail */
 
 	bool rc_signal_found_once;
 	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;
@@ -217,7 +211,7 @@ struct vehicle_status_s
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;
 	uint32_t onboard_control_sensors_health;
-	
+
 	float load;					/**< processor load from 0 to 1 */
 	float battery_voltage;
 	float battery_current;
@@ -230,6 +224,8 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
+
+	bool circuit_breaker_engaged_power_check;
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h
index 0822fa89a..e19a34a5d 100644
--- a/src/modules/uORB/topics/vehicle_vicon_position.h
+++ b/src/modules/uORB/topics/vehicle_vicon_position.h
@@ -52,8 +52,7 @@
 /**
  * Fused local position in NED.
  */
-struct vehicle_vicon_position_s
-{
+struct vehicle_vicon_position_s {
 	uint64_t timestamp;			/**< time of this estimate, in microseconds since system start */
 	bool valid;				/**< true if position satisfies validity criteria of estimator */
 
diff --git a/src/modules/uORB/topics/wind_estimate.h b/src/modules/uORB/topics/wind_estimate.h
new file mode 100644
index 000000000..58333a64f
--- /dev/null
+++ b/src/modules/uORB/topics/wind_estimate.h
@@ -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 wind_estimate.h
+ *
+ * Wind estimate topic topic
+ *
+ */
+
+#ifndef TOPIC_WIND_ESTIMATE_H
+#define TOPIC_WIND_ESTIMATE_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/** Wind estimate */
+struct wind_estimate_s {
+
+	uint64_t	timestamp;		/**< Microseconds since system boot */
+	float		windspeed_north;	/**< Wind component in north / X direction */
+	float		windspeed_east;		/**< Wind component in east / Y direction */
+	float		covariance_north;	/**< Uncertainty - set to zero (no uncertainty) if not estimated */
+	float		covariance_east;	/**< Uncertainty - set to zero (no uncertainty) if not estimated */
+};
+
+/**
+ * @}
+ */
+
+ORB_DECLARE(wind_estimate);
+
+#endif
\ No newline at end of file
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
index 64ee544a2..02d1af481 100644
--- a/src/modules/unit_test/unit_test.cpp
+++ b/src/modules/unit_test/unit_test.cpp
@@ -32,17 +32,10 @@
  *
  ****************************************************************************/
 
-/**
- * @file unit_test.cpp
- * A unit test library.
- *
- */
-
 #include "unit_test.h"
 
 #include <systemlib/err.h>
 
-
 UnitTest::UnitTest()
 {
 }
@@ -51,15 +44,15 @@ UnitTest::~UnitTest()
 {
 }
 
-void
-UnitTest::print_results(const char* result)
+void UnitTest::printResults(void)
+{
+    warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
+    warnx("  Tests passed : %d", mu_tests_passed());
+    warnx("  Tests failed : %d", mu_tests_failed());
+    warnx("  Assertions : %d", mu_assertion());
+}
+
+void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line)
 {
-	if (result != 0) {
-        	warnx("Failed: %s:%d", mu_last_test(), mu_line());
-        	warnx("%s", result);
-    	} else {
-        	warnx("ALL TESTS PASSED");
-        	warnx("  Tests run : %d", mu_tests_run());
-        	warnx("  Assertion : %d", mu_assertion());
-    	}
+    warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
 }
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index 3020734f4..32eb8c308 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -32,62 +32,55 @@
  *
  ****************************************************************************/
 
-/**
- * @file unit_test.h
- * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html).
- *
- */
-
 #ifndef UNIT_TEST_H_
-#define UNIT_TEST_
+#define UNIT_TEST_H_
 
 #include <systemlib/err.h>
 
-
 class __EXPORT UnitTest
 {
 
 public:
-#define xstr(s) str(s)
-#define str(s) #s
 #define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
 
 INLINE_GLOBAL(int, mu_tests_run)
+INLINE_GLOBAL(int, mu_tests_failed)
+INLINE_GLOBAL(int, mu_tests_passed)
 INLINE_GLOBAL(int, mu_assertion)
 INLINE_GLOBAL(int, mu_line)
 INLINE_GLOBAL(const char*, mu_last_test)
 
-#define mu_assert(message, test)                                           \
-    do                                                                     \
-    {                                                                      \
-        if (!(test))                                                       \
-            return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \
-        else                                                               \
-            mu_assertion()++;                                              \
-    } while (0)
-
-
-#define mu_run_test(test)                                                  \
-do                                                                         \
-{                                                                          \
-    const char *message;                                                   \
-    mu_last_test() = #test;                                                \
-    mu_line() = __LINE__;                                                  \
-    message = test();                                                      \
-    mu_tests_run()++;                                                      \
-    if (message)                                                           \
-        return message;                                                    \
-} while (0)
-
-
-public:
 	UnitTest();
     virtual ~UnitTest();
 
-    virtual const char*     run_tests() = 0;
-    virtual void            print_results(const char* result);
-};
-
+    virtual void runTests(void) = 0;
+    void printResults(void);
+    
+    void printAssert(const char* msg, const char* test, const char* file, int line);
+    
+#define ut_assert(message, test)                                \
+    do {                                                        \
+        if (!(test)) {                                          \
+            printAssert(message, #test, __FILE__, __LINE__);    \
+            return false;                                       \
+        } else {                                                \
+            mu_assertion()++;                                   \
+        }                                                       \
+    } while (0)
+    
+#define ut_run_test(test)                       \
+    do {                                        \
+        warnx("RUNNING TEST: %s", #test);       \
+        mu_tests_run()++;                       \
+        if (!test()) {                          \
+            warnx("TEST FAILED: %s", #test);    \
+            mu_tests_failed()++;                \
+        } else {                                \
+            warnx("TEST PASSED: %s", #test);    \
+            mu_tests_passed()++;                \
+        }                                       \
+    } while (0)
 
+};
 
 #endif /* UNIT_TEST_H_ */
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 476015f3e..4a97d328c 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.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
@@ -94,7 +92,6 @@ do_device(int argc, char *argv[])
 	}
 
 	int	fd;
-	int	ret;
 
 	fd = open(argv[0], 0);
 
@@ -104,6 +101,8 @@ do_device(int argc, char *argv[])
 
 	} else {
 
+		int	ret;
+
 		if (argc == 2 && !strcmp(argv[1], "block")) {
 
 			/* disable the device publications */
@@ -121,7 +120,7 @@ do_device(int argc, char *argv[])
 				errx(ret,"uORB publications could not be unblocked");
 
 		} else {
-			errx("no valid command: %s", argv[1]);
+			errx(1, "no valid command: %s", argv[1]);
 		}
 	}
 
@@ -132,7 +131,6 @@ static void
 do_gyro(int argc, char *argv[])
 {
 	int	fd;
-	int	ret;
 
 	fd = open(GYRO_DEVICE_PATH, 0);
 
@@ -142,6 +140,8 @@ do_gyro(int argc, char *argv[])
 
 	} else {
 
+		int ret;
+
 		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
 			/* set the gyro internal sampling rate up to at least i Hz */
@@ -173,8 +173,13 @@ do_gyro(int argc, char *argv[])
 				warnx("gyro self test FAILED! Check calibration:");
 				struct gyro_scale scale;
 				ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
-				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+
+				if (ret) {
+					err(1, "failed getting gyro scale");
+				}
+
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
 			} else {
 				warnx("gyro calibration and self test OK");
 			}
@@ -199,7 +204,6 @@ static void
 do_mag(int argc, char *argv[])
 {
 	int fd;
-	int ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -209,6 +213,8 @@ do_mag(int argc, char *argv[])
 
 	} else {
 
+		int ret;
+
 		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
 			/* set the mag internal sampling rate up to at least i Hz */
@@ -240,8 +246,13 @@ do_mag(int argc, char *argv[])
 				warnx("mag self test FAILED! Check calibration:");
 				struct mag_scale scale;
 				ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
-				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+
+				if (ret) {
+					err(ret, "failed getting mag scale");
+				}
+
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
 			} else {
 				warnx("mag calibration and self test OK");
 			}
@@ -266,7 +277,6 @@ static void
 do_accel(int argc, char *argv[])
 {
 	int	fd;
-	int	ret;
 
 	fd = open(ACCEL_DEVICE_PATH, 0);
 
@@ -276,6 +286,8 @@ do_accel(int argc, char *argv[])
 
 	} else {
 
+		int ret;
+
 		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
 			/* set the accel internal sampling rate up to at least i Hz */
@@ -307,8 +319,13 @@ do_accel(int argc, char *argv[])
 				warnx("accel self test FAILED! Check calibration:");
 				struct accel_scale scale;
 				ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
-				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+
+				if (ret) {
+					err(ret, "failed getting accel scale");
+				}
+
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
 			} else {
 				warnx("accel calibration and self test OK");
 			}
diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/systemcmds/dumpfile/dumpfile.c
index b72bbb2b1..c18814342 100644
--- a/src/modules/mavlink_onboard/mavlink_bridge_header.h
+++ b/src/systemcmds/dumpfile/dumpfile.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -33,51 +32,85 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_bridge_header
- * MAVLink bridge header for UART access.
+ * @file dumpfile.c
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-/* MAVLink adapter header */
-#ifndef MAVLINK_BRIDGE_HEADER_H
-#define MAVLINK_BRIDGE_HEADER_H
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <termios.h>
 
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+#include <systemlib/err.h>
 
-/* use efficient approach, see mavlink_helpers.h */
-#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
+__EXPORT int dumpfile_main(int argc, char *argv[]);
 
-#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
-#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
+int
+dumpfile_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "usage: dumpfile <filename>");
+	}
 
-#include <v1.0/mavlink_types.h>
-#include <unistd.h>
+	/* open input file */
+	FILE *f;
+	f = fopen(argv[1], "r");
 
+	if (f == NULL) {
+		printf("ERROR\n");
+		exit(1);
+	}
 
-/* Struct that stores the communication settings of this system.
-   you can also define / alter these settings elsewhere, as long
-   as they're included BEFORE mavlink.h.
-   So you can set the
+	/* get file size */
+	fseek(f, 0L, SEEK_END);
+	int size = ftell(f);
+	fseek(f, 0L, SEEK_SET);
 
-   mavlink_system.sysid = 100; // System ID, 1-255
-   mavlink_system.compid = 50; // Component/Subsystem ID, 1-255
+	printf("OK %d\n", size);
 
-   Lines also in your main.c, e.g. by reading these parameter from EEPROM.
- */
-extern mavlink_system_t mavlink_system;
+	/* configure stdout */
+	int out = fileno(stdout);
 
-/**
- * @brief Send multiple chars (uint8_t) over a comm channel
- *
- * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
- * @param ch Character to send
- */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
+	struct termios tc;
+	struct termios tc_old;
+	tcgetattr(out, &tc);
+
+	/* save old terminal attributes to restore it later on exit */
+	memcpy(&tc_old, &tc, sizeof(tc));
+
+	/* don't add CR on each LF*/
+	tc.c_oflag &= ~ONLCR;
+
+	if (tcsetattr(out, TCSANOW, &tc) < 0) {
+		warnx("ERROR setting stdout attributes");
+		exit(1);
+	}
+
+	char buf[512];
+	int nread;
+
+	/* dump file */
+	while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) {
+		if (write(out, buf, nread) <= 0) {
+			warnx("error dumping file");
+			break;
+		}
+	}
 
-mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
-mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
+	fsync(out);
+	fclose(f);
 
-#include <v1.0/common/mavlink.h>
+	/* restore old terminal attributes */
+	if (tcsetattr(out, TCSANOW, &tc_old) < 0) {
+		warnx("ERROR restoring stdout attributes");
+		exit(1);
+	}
 
-#endif /* MAVLINK_BRIDGE_HEADER_H */
+	return OK;
+}
diff --git a/src/systemcmds/dumpfile/module.mk b/src/systemcmds/dumpfile/module.mk
new file mode 100644
index 000000000..36461f477
--- /dev/null
+++ b/src/systemcmds/dumpfile/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Dump file utility
+#
+
+MODULE_COMMAND	 = dumpfile
+SRCS		 = dumpfile.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index ad1996694..7d80af307 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -182,7 +182,7 @@ esc_calib_main(int argc, char *argv[])
 
 	if (orb_updated) {
 		errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
-			"\tmultirotor_att_control stop\n"
+			"\tmc_att_control stop\n"
 			"\tfw_att_control stop\n");
 	}
 
diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp
index 6da39d371..2f5ed3265 100644
--- a/src/systemcmds/mixer/mixer.cpp
+++ b/src/systemcmds/mixer/mixer.cpp
@@ -102,7 +102,8 @@ load(const char *devname, const char *fname)
 	if (ioctl(dev, MIXERIOCRESET, 0))
 		err(1, "can't reset mixers on %s", devname);
 
-	load_mixer_file(fname, &buf[0], sizeof(buf));
+	if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0)
+		err(1, "can't load mixer: %s", fname);
 
 	/* XXX pass the buffer to the device */
 	int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c
index e34be44e3..991363797 100644
--- a/src/systemcmds/mtd/24xxxx_mtd.c
+++ b/src/systemcmds/mtd/24xxxx_mtd.c
@@ -142,12 +142,9 @@ struct at24c_dev_s {
 	uint16_t              pagesize; /* 32, 63 */
 	uint16_t              npages;   /* 128, 256, 512, 1024 */
 
-	perf_counter_t        perf_reads;
-	perf_counter_t        perf_writes;
-	perf_counter_t        perf_resets;
-	perf_counter_t        perf_read_retries;
-	perf_counter_t        perf_read_errors;
-	perf_counter_t        perf_write_errors;
+	perf_counter_t        perf_transfers;
+	perf_counter_t        perf_resets_retries;
+	perf_counter_t        perf_errors;
 };
 
 /************************************************************************************
@@ -164,6 +161,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock,
 static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg);
 
 void at24c_test(void);
+int at24c_nuke(void);
 
 /************************************************************************************
  * Private Data
@@ -240,8 +238,10 @@ void at24c_test(void)
 		} else if (result != 1) {
 			vdbg("unexpected %u\n", result);
 		}
-		if ((count % 100) == 0)
+
+		if ((count % 100) == 0) {
 			vdbg("test %u errors %u\n", count, errors);
+		}
 	}
 }
 
@@ -298,9 +298,9 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
 
 		for (;;) {
 
-			perf_begin(priv->perf_reads);
+			perf_begin(priv->perf_transfers);
 			ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
-			perf_end(priv->perf_reads);
+			perf_end(priv->perf_transfers);
 
 			if (ret >= 0)
 				break;
@@ -314,10 +314,10 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
 			 * XXX maybe do special first-read handling with optional
 			 * bus reset as well?
 			 */
-			perf_count(priv->perf_read_retries);
+			perf_count(priv->perf_resets_retries);
 
 			if (--tries == 0) {
-				perf_count(priv->perf_read_errors);
+				perf_count(priv->perf_errors);
 				return ERROR;
 			}
 		}
@@ -383,9 +383,9 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t
 
 		for (;;) {
 
-			perf_begin(priv->perf_writes);
+			perf_begin(priv->perf_transfers);
 			ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
-			perf_end(priv->perf_writes);
+			perf_end(priv->perf_transfers);
 
 			if (ret >= 0)
 				break;
@@ -397,7 +397,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t
 			 * poll for write completion.
 			 */
 			if (--tries == 0) {
-				perf_count(priv->perf_write_errors);
+				perf_count(priv->perf_errors);
 				return ERROR;
 			}
 		}
@@ -521,12 +521,9 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
 		priv->mtd.ioctl  = at24c_ioctl;
 		priv->dev        = dev;
 
-		priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read");
-		priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write");
-		priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset");
-		priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries");
-		priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors");
-		priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
+		priv->perf_transfers = perf_alloc(PC_ELAPSED, "eeprom_trans");
+		priv->perf_resets_retries = perf_alloc(PC_COUNT, "eeprom_rst");
+		priv->perf_errors = perf_alloc(PC_COUNT, "eeprom_errs");
 	}
 
 	/* attempt to read to validate device is present */
@@ -548,9 +545,9 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
 		}
 	};
 
-	perf_begin(priv->perf_reads);
+	perf_begin(priv->perf_transfers);
 	int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
-	perf_end(priv->perf_reads);
+	perf_end(priv->perf_transfers);
 
 	if (ret < 0) {
 		return NULL;
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index 0a88d40f3..a925cdd40 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -91,7 +91,7 @@ static void	mtd_test(void);
 static void	mtd_erase(char *partition_names[], unsigned n_partitions);
 static void	mtd_readtest(char *partition_names[], unsigned n_partitions);
 static void	mtd_rwtest(char *partition_names[], unsigned n_partitions);
-static void	mtd_print_info();
+static void	mtd_print_info(void);
 static int	mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, 
 	unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
 
@@ -104,6 +104,16 @@ static unsigned n_partitions_current = 0;
 static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"};
 static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]);
 
+static void
+mtd_status(void)
+{
+	if (!attached)
+		errx(1, "MTD driver not started");
+    
+	mtd_print_info();
+	exit(0);
+}
+
 int mtd_main(int argc, char *argv[])
 {
 	if (argc >= 2) {
@@ -160,7 +170,11 @@ static void
 ramtron_attach(void)
 {
 	/* find the right spi */
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	struct spi_dev_s *spi = up_spiinitialize(4);
+#else
 	struct spi_dev_s *spi = up_spiinitialize(2);
+#endif
 	/* this resets the spi bus, set correct bus speed again */
 	SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
 	SPI_SETBITS(spi, 8);
@@ -189,8 +203,12 @@ ramtron_attach(void)
 		errx(1, "failed to initialize mtd driver");
 
 	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
-        if (ret != OK)
-            warnx(1, "failed to set bus speed");
+	if (ret != OK) {
+		// FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried
+		// that but setting the bug speed does fail all the time. Which was then exiting and the board would
+		// not run correctly. So changed to warnx.
+		warnx("failed to set bus speed");
+	}
 
 	attached = true;
 }
@@ -347,7 +365,7 @@ static ssize_t mtd_get_partition_size(void)
 	return partsize;
 }
 
-void mtd_print_info()
+void mtd_print_info(void)
 {
 	if (!attached)
 		exit(1);
@@ -378,16 +396,6 @@ mtd_test(void)
 }
 
 void
-mtd_status(void)
-{
-	if (!attached)
-		errx(1, "MTD driver not started");
-
-	mtd_print_info();
-	exit(0);
-}
-
-void
 mtd_erase(char *partition_names[], unsigned n_partitions)
 {
 	uint8_t v[64];
@@ -420,7 +428,7 @@ mtd_readtest(char *partition_names[], unsigned n_partitions)
 
 	uint8_t v[128];
 	for (uint8_t i = 0; i < n_partitions; i++) {
-		uint32_t count = 0;
+		ssize_t count = 0;
 		printf("reading %s expecting %u bytes\n", partition_names[i], expected_size);
 		int fd = open(partition_names[i], O_RDONLY);
 		if (fd == -1) {
@@ -451,8 +459,8 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions)
 
 	uint8_t v[128], v2[128];
 	for (uint8_t i = 0; i < n_partitions; i++) {
-		uint32_t count = 0;
-                off_t offset = 0;
+		ssize_t count = 0;
+        off_t offset = 0;
 		printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size);
 		int fd = open(partition_names[i], O_RDWR);
 		if (fd == -1) {
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
index a48588535..b22b446da 100644
--- a/src/systemcmds/nshterm/module.mk
+++ b/src/systemcmds/nshterm/module.mk
@@ -38,4 +38,4 @@
 MODULE_COMMAND	 = nshterm
 SRCS		 = nshterm.c
 
-MODULE_STACKSIZE = 3000
+MODULE_STACKSIZE = 1200
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
index 7d9484d3e..fca1798e6 100644
--- a/src/systemcmds/nshterm/nshterm.c
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -60,7 +60,7 @@ nshterm_main(int argc, char *argv[])
         printf("Usage: nshterm <device>\n");
         exit(1);
     }
-    uint8_t retries = 0;
+    unsigned retries = 0;
     int fd = -1;
 
     /* try the first 30 seconds */
diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk
index 63f15ad28..f716eb71e 100644
--- a/src/systemcmds/param/module.mk
+++ b/src/systemcmds/param/module.mk
@@ -38,7 +38,8 @@
 MODULE_COMMAND	 = param
 SRCS		 = param.c
 
-MODULE_STACKSIZE = 4096
+# Note: measurements yielded a max of 900 bytes used.
+MODULE_STACKSIZE = 1800
 
 MAXOPTIMIZATION	 = -Os
 
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 0cbba0a37..e110335e7 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -46,6 +46,7 @@
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <math.h>
 #include <sys/stat.h>
 
 #include <arch/board/board.h>
@@ -61,8 +62,10 @@ static void	do_load(const char* param_file_name);
 static void	do_import(const char* param_file_name);
 static void	do_show(const char* search_string);
 static void	do_show_print(void *arg, param_t param);
-static void	do_set(const char* name, const char* val);
-static void	do_compare(const char* name, const char* vals[], unsigned comparisons);
+static void	do_set(const char* name, const char* val, bool fail_on_not_found);
+static void	do_compare(const char* name, char* vals[], unsigned comparisons);
+static void	do_reset(void);
+static void	do_reset_nostart(void);
 
 int
 param_main(int argc, char *argv[])
@@ -116,10 +119,17 @@ param_main(int argc, char *argv[])
 		}
 
 		if (!strcmp(argv[1], "set")) {
-			if (argc >= 4) {
-				do_set(argv[2], argv[3]);
+			if (argc >= 5) {
+
+				/* if the fail switch is provided, fails the command if not found */
+				bool fail = !strcmp(argv[4], "fail");
+
+				do_set(argv[2], argv[3], fail);
+
+			} else if (argc >= 4) {
+				do_set(argv[2], argv[3], false);
 			} else {
-				errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'");
+				errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'");
 			}
 		}
 
@@ -130,6 +140,14 @@ param_main(int argc, char *argv[])
 				errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'");
 			}
 		}
+
+		if (!strcmp(argv[1], "reset")) {
+			do_reset();
+		}
+
+		if (!strcmp(argv[1], "reset_nostart")) {
+			do_reset_nostart();
+		}
 	}
 	
 	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
@@ -211,9 +229,8 @@ do_show_print(void *arg, param_t param)
 	if (!(arg == NULL)) {
 
 		/* start search */
-		char *ss = search_string;
-		char *pp = p_name;
-		bool mismatch = false;
+		const char *ss = search_string;
+		const char *pp = p_name;
 
 		/* XXX this comparison is only ok for trailing wildcards */
 		while (*ss != '\0' && *pp != '\0') {
@@ -277,7 +294,7 @@ do_show_print(void *arg, param_t param)
 }
 
 static void
-do_set(const char* name, const char* val)
+do_set(const char* name, const char* val, bool fail_on_not_found)
 {
 	int32_t i;
 	float f;
@@ -285,8 +302,8 @@ do_set(const char* name, const char* val)
 
 	/* set nothing if parameter cannot be found */
 	if (param == PARAM_INVALID) {
-		/* param not found */
-		errx(1, "Error: Parameter %s not found.", name);
+		/* param not found - fail silenty in scripts as it prevents booting */
+		errx(((fail_on_not_found) ? 1 : 0), "Error: Parameter %s not found.", name);
 	}
 
 	printf("%c %s: ",
@@ -334,7 +351,7 @@ do_set(const char* name, const char* val)
 }
 
 static void
-do_compare(const char* name, const char* vals[], unsigned comparisons)
+do_compare(const char* name, char* vals[], unsigned comparisons)
 {
 	int32_t i;
 	float f;
@@ -402,3 +419,39 @@ do_compare(const char* name, const char* vals[], unsigned comparisons)
 
 	exit(ret);
 }
+
+static void
+do_reset(void)
+{
+	param_reset_all();
+
+	if (param_save_default()) {
+		warnx("Param export failed.");
+		exit(1);
+	} else {
+		exit(0);
+	}
+}
+
+static void
+do_reset_nostart(void)
+{
+
+	int32_t autostart;
+	int32_t autoconfig;
+
+	(void)param_get(param_find("SYS_AUTOSTART"), &autostart);
+	(void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig);
+
+	param_reset_all();
+
+	(void)param_set(param_find("SYS_AUTOSTART"), &autostart);
+	(void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig);
+
+	if (param_save_default()) {
+		warnx("Param export failed.");
+		exit(1);
+	} else {
+		exit(0);
+	}
+}
diff --git a/src/systemcmds/perf/module.mk b/src/systemcmds/perf/module.mk
index 77952842b..ec39a7a85 100644
--- a/src/systemcmds/perf/module.mk
+++ b/src/systemcmds/perf/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 = perf
 SRCS		 = perf.c
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1800
diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c
index b69ea597b..b08a2e3b7 100644
--- a/src/systemcmds/perf/perf.c
+++ b/src/systemcmds/perf/perf.c
@@ -73,7 +73,7 @@ int perf_main(int argc, char *argv[])
 		return -1;
 	}
 
-	perf_print_all();
+	perf_print_all(0 /* stdout */);
 	fflush(stdout);
 	return 0;
 }
diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk
index 7c3c88783..0cb2a4cd0 100644
--- a/src/systemcmds/preflight_check/module.mk
+++ b/src/systemcmds/preflight_check/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	 = preflight_check
 SRCS		 = preflight_check.c
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1800
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 982b03782..86e4ff545 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -44,6 +44,7 @@
 #include <string.h>
 #include <fcntl.h>
 #include <errno.h>
+#include <math.h>
 
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
@@ -87,9 +88,7 @@ int preflight_check_main(int argc, char *argv[])
 	/* give the system some time to sample the sensors in the background */
 	usleep(150000);
 
-
 	/* ---- MAG ---- */
-	close(fd);
 	fd = open(MAG_DEVICE_PATH, 0);
 	if (fd < 0) {
 		warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk
index 4a23bba90..13a24150f 100644
--- a/src/systemcmds/pwm/module.mk
+++ b/src/systemcmds/pwm/module.mk
@@ -38,4 +38,4 @@
 MODULE_COMMAND	 = pwm
 SRCS		 = pwm.c
 
-MODULE_STACKSIZE = 4096
+MODULE_STACKSIZE = 1800
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 7c23f38c2..e0e6ca537 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -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
@@ -443,6 +443,111 @@ pwm_main(int argc, char *argv[])
 					exit(0);
 				}
 			}
+			usleep(2000);
+		}
+		exit(0);
+
+
+	} else if (!strcmp(argv[1], "steps")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+
+		/* get current servo values */
+		struct pwm_output_values last_spos;
+
+		for (unsigned i = 0; i < servo_count; i++) {
+
+			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
+			if (ret != OK)
+				err(1, "PWM_SERVO_GET(%d)", i);
+		}
+
+		/* perform PWM output */
+
+		/* Open console directly to grab CTRL-C signal */
+		struct pollfd fds;
+		fds.fd = 0; /* stdin */
+		fds.events = POLLIN;
+
+		warnx("Running 5 steps. WARNING! Motors will be live in 5 seconds\nPress any key to abort now.");
+		sleep(5);
+
+		unsigned off = 900;
+		unsigned idle = 1300;
+		unsigned full = 2000;
+		unsigned steps_timings_us[] = {2000, 5000, 20000, 50000};
+
+		unsigned phase = 0;
+		unsigned phase_counter = 0;
+		unsigned const phase_maxcount = 20;
+
+		for (	unsigned steps_timing_index = 0;
+			steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
+			steps_timing_index++ ) {
+
+			warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]);
+
+			while (1) {
+				for (unsigned i = 0; i < servo_count; i++) {
+					if (set_mask & 1<<i) {
+
+						unsigned val;
+
+						if (phase == 0) {
+							val = idle;
+						} else if (phase == 1) {
+							/* ramp - depending how steep it is this ramp will look instantaneous on the output */
+							val = idle + (full - idle) * (phase_maxcount / (float)phase_counter);
+						} else {
+							val = off;
+						}
+
+						ret = ioctl(fd, PWM_SERVO_SET(i), val);
+						if (ret != OK)
+							err(1, "PWM_SERVO_SET(%d)", i);
+					}
+				}
+
+				/* abort on user request */
+				char c;
+				ret = poll(&fds, 1, 0);
+				if (ret > 0) {
+
+					ret = read(0, &c, 1);
+
+					if (ret > 0) {
+						/* reset output to the last value */
+						for (unsigned i = 0; i < servo_count; i++) {
+										if (set_mask & 1<<i) {
+											ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
+											if (ret != OK)
+												err(1, "PWM_SERVO_SET(%d)", i);
+										}
+									}
+						warnx("Key pressed, user abort\n");
+						exit(0);
+					}
+				}
+				if (phase == 1) {
+					usleep(steps_timings_us[steps_timing_index] / phase_maxcount);
+
+				} else if (phase == 0) {
+					usleep(50000);
+				} else if (phase == 2) {
+					usleep(50000);
+				} else {
+					break;
+				}
+
+				phase_counter++;
+
+				if (phase_counter > phase_maxcount) {
+					phase++;
+					phase_counter = 0;
+				}
+			}
 		}
 		exit(0);
 
diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk
index 19f64af54..edf9d8b37 100644
--- a/src/systemcmds/reboot/module.mk
+++ b/src/systemcmds/reboot/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 = reboot
 SRCS		 = reboot.c
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 800
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index beb9ad13d..622a0faf3 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -24,7 +24,9 @@ SRCS			 = test_adc.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
 			   test_mixer.cpp \
+			   test_mathlib.cpp \
 			   test_file.c \
+			   test_file2.c \
 			   tests_main.c \
 			   test_param.c \
 			   test_ppm_loopback.c \
diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c
index 030ac6e23..03391b851 100644
--- a/src/systemcmds/tests/test_adc.c
+++ b/src/systemcmds/tests/test_adc.c
@@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[])
 	}
 
 	for (unsigned i = 0; i < 5; i++) {
-		/* make space for a maximum of eight channels */
-		struct adc_msg_s data[8];
+		/* make space for a maximum of twelve channels */
+		struct adc_msg_s data[12];
 		/* read all channels available */
 		ssize_t count = read(fd, data, sizeof(data));
 
diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c
index 6130fe763..12d598df4 100644
--- a/src/systemcmds/tests/test_bson.c
+++ b/src/systemcmds/tests/test_bson.c
@@ -40,6 +40,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
+#include <math.h>
 
 #include <systemlib/err.h>
 #include <systemlib/bson/tinybson.h>
@@ -123,7 +124,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
 			warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE);
 			return 1;
 		}
-		if (node->d != sample_double) {
+		if (fabs(node->d - sample_double) > 1e-12) {
 			warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double);
 			return 1;
 		}
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
index 50dece816..fda949c61 100644
--- a/src/systemcmds/tests/test_conv.cpp
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[])
 		float f = i/10000.0f;
 		float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
 		if (fabsf(f - fres) > 0.0001f) {
-			warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
+			warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres);
 			return 1;
 		}
 	}
diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c
new file mode 100644
index 000000000..1f844a97d
--- /dev/null
+++ b/src/systemcmds/tests/test_dataman.c
@@ -0,0 +1,182 @@
+/****************************************************************************
+ * px4/sensors/test_dataman.c
+ *
+ *  Copyright (C) 2012 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_led.h>
+#include <systemlib/systemlib.h>
+#include <drivers/drv_hrt.h>
+#include <semaphore.h>
+
+
+#include "tests.h"
+
+#include "dataman/dataman.h"
+
+static sem_t *sems;
+
+static int
+task_main(int argc, char *argv[])
+{
+	char buffer[DM_MAX_DATA_SIZE];
+	hrt_abstime wstart, wend, rstart, rend;
+
+	warnx("Starting dataman test task %s", argv[1]);
+	/* try to read an invalid item */
+	int my_id = atoi(argv[1]);
+	/* try to read an invalid item */
+	if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
+		warnx("%d read an invalid item failed", my_id);
+		goto fail;
+	}
+	/* try to read an invalid index */
+	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
+		warnx("%d read an invalid index failed", my_id);
+		goto fail;
+	}
+	srand(hrt_absolute_time() ^ my_id);
+	unsigned hit = 0, miss = 0;
+	wstart = hrt_absolute_time();
+	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		memset(buffer, my_id, sizeof(buffer));
+		buffer[1] = i;
+		unsigned hash = i ^ my_id;
+		unsigned len = (hash & 63) + 2;
+
+		int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
+		warnx("ret: %d", ret);
+		if (ret != len) {
+			warnx("%d write failed, index %d, length %d", my_id, hash, len);
+			goto fail;
+		}
+		usleep(rand() & ((64 * 1024) - 1));
+	}
+	rstart = hrt_absolute_time();
+	wend = rstart;
+
+	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		unsigned hash = i ^ my_id;
+		unsigned len2, len = (hash & 63) + 2;
+		if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) {
+			warnx("%d read failed length test, index %d", my_id, hash);
+			goto fail;
+		}
+		if (buffer[0] == my_id) {
+			hit++;
+			if (len2 != len) {
+				warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2);
+				goto fail;
+			}
+			if (buffer[1] != i) {
+				warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
+				goto fail;
+			}
+		}
+		else
+			miss++;
+	}
+	rend = hrt_absolute_time();
+	warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
+		my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
+	sem_post(sems + my_id);
+	return 0;
+fail:
+	warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x",
+		my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
+	sem_post(sems + my_id);
+	return -1;
+}
+
+int test_dataman(int argc, char *argv[])
+{
+	int i, num_tasks = 4;
+	char buffer[DM_MAX_DATA_SIZE];
+
+	if (argc > 1)
+		num_tasks = atoi(argv[1]);
+	
+	sems = (sem_t *)malloc(num_tasks * sizeof(sem_t));
+	warnx("Running %d tasks", num_tasks);
+	for (i = 0; i < num_tasks; i++) {
+		int task;
+		char a[16];
+		sprintf(a, "%d", i);
+		const char *av[2];
+		av[0] = a;
+		av[1] = 0;
+		sem_init(sems + i, 1, 0);
+		/* start the task */
+		if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) {
+			warn("task start failed");
+		}
+	}
+	for (i = 0; i < num_tasks; i++) {
+		sem_wait(sems + i);
+		sem_destroy(sems + i);
+	}
+	free(sems);
+	dm_restart(DM_INIT_REASON_IN_FLIGHT);
+	for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0)
+			break;
+	}
+	if (i >= NUM_MISSIONS_SUPPORTED) {
+		warnx("Restart in-flight failed");
+		return -1;
+
+	}
+	dm_restart(DM_INIT_REASON_POWER_ON);
+	for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
+			warnx("Restart power-on failed");
+			return -1;
+		}
+	}
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c
index 96be1e8df..570583dee 100644
--- a/src/systemcmds/tests/test_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -102,7 +102,7 @@ test_file(int argc, char *argv[])
 	}
 
 	/* perform tests for a range of chunk sizes */
-	unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
+	int chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
 
 	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
 
@@ -116,7 +116,7 @@ test_file(int argc, char *argv[])
 			uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 
 			/* fill write buffer with known values */
-			for (int i = 0; i < sizeof(write_buf); i++) {
+			for (size_t i = 0; i < sizeof(write_buf); i++) {
 				/* this will wrap, but we just need a known value with spacing */
 				write_buf[i] = i+11;
 			}
@@ -149,6 +149,8 @@ test_file(int argc, char *argv[])
 			}
 			end = hrt_absolute_time();
 
+			warnx("write took %llu us", (end - start));
+
 			close(fd);
 			fd = open("/fs/microsd/testfile", O_RDONLY);
 
@@ -192,7 +194,6 @@ test_file(int argc, char *argv[])
 
 			warnx("testing aligned writes - please wait.. (CTRL^C to abort)");
 
-			start = hrt_absolute_time();
 			for (unsigned i = 0; i < iterations; i++) {
 				int wret = write(fd, write_buf, chunk_sizes[c]);
 
@@ -224,9 +225,6 @@ test_file(int argc, char *argv[])
 					return 1;
 				}
 				
-				/* compare value */
-				bool compare_ok = true;
-
 				for (int j = 0; j < chunk_sizes[c]; j++) {
 					if (read_buf[j] != write_buf[j]) {
 						warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]);
diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c
new file mode 100644
index 000000000..8db3ea5ae
--- /dev/null
+++ b/src/systemcmds/tests/test_file2.c
@@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 test_file2.c
+ *
+ * File write test.
+ */
+
+#include <sys/stat.h>
+#include <dirent.h>
+#include <stdio.h>
+#include <stddef.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+#include <stdlib.h>
+#include <getopt.h>
+
+#include "tests.h"
+
+#define FLAG_FSYNC 1
+#define FLAG_LSEEK 2
+
+/*
+  return a predictable value for any file offset to allow detection of corruption
+ */
+static uint8_t get_value(uint32_t ofs)
+{
+	union {
+		uint32_t ofs;
+		uint8_t buf[4];
+	} u;
+	u.ofs = ofs;
+	return u.buf[ofs % 4];
+}
+
+static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags)
+{
+	printf("Testing on %s with write_chunk=%u write_size=%u\n", 
+	       filename, (unsigned)write_chunk, (unsigned)write_size);
+	
+	uint32_t ofs = 0;
+	int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+	
+	// create a file of size write_size, in write_chunk blocks
+	uint8_t counter = 0;
+	while (ofs < write_size) {
+        uint8_t buffer[write_chunk];
+        for (uint16_t j=0; j<write_chunk; j++) {
+		buffer[j] = get_value(ofs);
+		ofs++;
+        }
+        if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
+            printf("write failed at offset %u\n", ofs);
+            exit(1);
+        }
+	if (flags & FLAG_FSYNC) {
+		fsync(fd);
+	}
+        if (counter % 100 == 0) {
+		printf("write ofs=%u\r", ofs);
+        }
+        counter++;
+	}
+	close(fd);
+	
+	printf("write ofs=%u\n", ofs);
+	
+	// read and check
+	fd = open(filename, O_RDONLY);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+
+	counter = 0;
+	ofs = 0;
+	while (ofs < write_size) {
+		uint8_t buffer[write_chunk];
+		if (counter % 100 == 0) {
+			printf("read ofs=%u\r", ofs);
+		}
+		counter++;
+		if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
+			printf("read failed at offset %u\n", ofs);
+			exit(1);
+		}
+		for (uint16_t j=0; j<write_chunk; j++) {
+			if (buffer[j] != get_value(ofs)) {
+				printf("corruption at ofs=%u got %u\n", ofs, buffer[j]);
+				exit(1);
+			}
+			ofs++;
+		}
+		if (flags & FLAG_LSEEK) {
+			lseek(fd, 0, SEEK_CUR);
+		}
+	}
+	printf("read ofs=%u\n", ofs);
+	close(fd);
+	unlink(filename);
+	printf("All OK\n");    
+}
+
+static void usage(void)
+{
+	printf("test file2 [options] [filename]\n");
+	printf("\toptions:\n");
+	printf("\t-s SIZE     set file size\n");
+	printf("\t-c CHUNK    set IO chunk size\n");
+	printf("\t-F          fsync on every write\n");
+	printf("\t-L          lseek on every read\n");
+}
+
+int test_file2(int argc, char *argv[])
+{
+	int opt;
+	uint16_t flags = 0;
+	const char *filename = "/fs/microsd/testfile2.dat";
+	uint32_t write_chunk = 64;
+	uint32_t write_size = 5*1024;
+
+	while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) {
+		switch (opt) {
+		case 'F':
+			flags |= FLAG_FSYNC;
+			break;
+		case 'L':
+			flags |= FLAG_LSEEK;
+			break;
+		case 's':
+			write_size = strtoul(optarg, NULL, 0);
+			break;
+		case 'c':
+			write_chunk = strtoul(optarg, NULL, 0);
+			break;
+		case 'h':
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+
+	if (argc > 0) {
+		filename = argv[0];
+	}
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+        test_corruption(filename, write_chunk, write_size, flags);
+        return 0;
+}
+
diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c
index 4921c9bbb..bb8b6c7f5 100644
--- a/src/systemcmds/tests/test_float.c
+++ b/src/systemcmds/tests/test_float.c
@@ -68,7 +68,7 @@ int test_float(int argc, char *argv[])
 	float sinf_one = sinf(1.0f);
 	float sqrt_two = sqrt(2.0f);
 
-	if (sinf_zero == 0.0f) {
+	if (fabsf(sinf_zero) < FLT_EPSILON) {
 		printf("\t success: sinf(0.0f) == 0.0f\n");
 
 	} else {
@@ -94,7 +94,7 @@ int test_float(int argc, char *argv[])
 		printf("\t success: asinf(1.0f) == 1.57079f\n");
 
 	} else {
-		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one);
+		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one);
 		ret = -1;
 	}
 
@@ -128,7 +128,7 @@ int test_float(int argc, char *argv[])
 
 	float sinf_zero_one = sinf(0.1f);
 
-	if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
+	if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
 		printf("\t success: sinf(0.1f) == 0.09983f\n");
 
 	} else {
@@ -136,7 +136,7 @@ int test_float(int argc, char *argv[])
 		ret = -2;
 	}
 
-	if (sqrt_two == 1.41421356f) {
+	if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) {
 		printf("\t success: sqrt(2.0f) == 1.41421f\n");
 
 	} else {
@@ -155,7 +155,7 @@ int test_float(int argc, char *argv[])
 	}
 
 	char sbuf[30];
-	sprintf(sbuf, "%8.4f", 0.553415f);
+	sprintf(sbuf, "%8.4f", (double)0.553415f);
 
 	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
 		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
@@ -166,7 +166,7 @@ int test_float(int argc, char *argv[])
 		ret = -5;
 	}
 
-	sprintf(sbuf, "%8.4f", -0.553415f);
+	sprintf(sbuf, "%8.4f", (double)-0.553415f);
 
 	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
 		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
@@ -188,7 +188,7 @@ int test_float(int argc, char *argv[])
 
 	double d1d2 = d1 * d2;
 
-	if (d1d2 == 2.022200000000000219557705349871) {
+	if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) {
 		printf("\t success: 1.0111 * 2.0 == 2.0222\n");
 
 	} else {
@@ -201,11 +201,11 @@ int test_float(int argc, char *argv[])
 	// Assign value of f1 to d1
 	d1 = f1;
 
-	if (f1 == (float)d1) {
+	if (fabsf(f1 - (float)d1) < FLT_EPSILON) {
 		printf("\t success: (float) 1.55f == 1.55 (double)\n");
 
 	} else {
-		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1);
+		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1);
 		ret = -8;
 	}
 
@@ -216,7 +216,7 @@ int test_float(int argc, char *argv[])
 	double sin_one = sin(1.0);
 	double atan2_ones = atan2(1.0, 1.0);
 
-	if (sin_zero == 0.0) {
+	if (fabs(sin_zero - 0.0) < DBL_EPSILON) {
 		printf("\t success: sin(0.0) == 0.0\n");
 
 	} else {
@@ -224,7 +224,7 @@ int test_float(int argc, char *argv[])
 		ret = -9;
 	}
 
-	if (sin_one == 0.841470984807896504875657228695) {
+	if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) {
 		printf("\t success: sin(1.0) == 0.84147098480\n");
 
 	} else {
@@ -232,7 +232,7 @@ int test_float(int argc, char *argv[])
 		ret = -10;
 	}
 
-	if (atan2_ones != 0.785398) {
+	if (fabs(atan2_ones - 0.785398) < DBL_EPSILON) {
 		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
 
 	} else {
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
new file mode 100644
index 000000000..70d173fc9
--- /dev/null
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ *   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 test_mathlib.cpp
+ *
+ * Mathlib test
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <time.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); }
+
+using namespace math;
+
+int test_mathlib(int argc, char *argv[])
+{
+	warnx("testing mathlib");
+
+	{
+		Vector<2> v;
+		Vector<2> v1(1.0f, 2.0f);
+		Vector<2> v2(1.0f, -1.0f);
+		float data[2] = {1.0f, 2.0f};
+		TEST_OP("Constructor Vector<2>()", Vector<2> v3);
+		TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1));
+		TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data));
+		TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f));
+		TEST_OP("Vector<2> = Vector<2>", v = v1);
+		TEST_OP("Vector<2> + Vector<2>", v + v1);
+		TEST_OP("Vector<2> - Vector<2>", v - v1);
+		TEST_OP("Vector<2> += Vector<2>", v += v1);
+		TEST_OP("Vector<2> -= Vector<2>", v -= v1);
+		TEST_OP("Vector<2> * Vector<2>", v * v1);
+		TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
+	}
+
+	{
+		Vector<3> v;
+		Vector<3> v1(1.0f, 2.0f, 0.0f);
+		Vector<3> v2(1.0f, -1.0f, 2.0f);
+		float data[3] = {1.0f, 2.0f, 3.0f};
+		TEST_OP("Constructor Vector<3>()", Vector<3> v3);
+		TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1));
+		TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data));
+		TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f));
+		TEST_OP("Vector<3> = Vector<3>", v = v1);
+		TEST_OP("Vector<3> + Vector<3>", v + v1);
+		TEST_OP("Vector<3> - Vector<3>", v - v1);
+		TEST_OP("Vector<3> += Vector<3>", v += v1);
+		TEST_OP("Vector<3> -= Vector<3>", v -= v1);
+		TEST_OP("Vector<3> * float", v1 * 2.0f);
+		TEST_OP("Vector<3> / float", v1 / 2.0f);
+		TEST_OP("Vector<3> *= float", v1 *= 2.0f);
+		TEST_OP("Vector<3> /= float", v1 /= 2.0f);
+		TEST_OP("Vector<3> * Vector<3>", v * v1);
+		TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
+		TEST_OP("Vector<3> length", v1.length());
+		TEST_OP("Vector<3> length squared", v1.length_squared());
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+		// Need pragma here intead of moving variable out of TEST_OP and just reference because
+		// TEST_OP measures performance of vector operations.
+		TEST_OP("Vector<3> element read", volatile float a = v1(0));
+		TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
+#pragma GCC diagnostic pop
+		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
+		TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
+	}
+
+	{
+		Vector<4> v;
+		Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
+		Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f);
+		float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
+		TEST_OP("Constructor Vector<4>()", Vector<4> v3);
+		TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1));
+		TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data));
+		TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f));
+		TEST_OP("Vector<4> = Vector<4>", v = v1);
+		TEST_OP("Vector<4> + Vector<4>", v + v1);
+		TEST_OP("Vector<4> - Vector<4>", v - v1);
+		TEST_OP("Vector<4> += Vector<4>", v += v1);
+		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
+		TEST_OP("Vector<4> * Vector<4>", v * v1);
+	}
+
+	{
+		Vector<10> v1;
+		v1.zero();
+		float data[10];
+		TEST_OP("Constructor Vector<10>()", Vector<10> v3);
+		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
+		TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
+	}
+
+	{
+		Matrix<3, 3> m1;
+		m1.identity();
+		Matrix<3, 3> m2;
+		m2.identity();
+		Vector<3> v1(1.0f, 2.0f, 0.0f);
+		TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1);
+		TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
+		TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
+	}
+
+	{
+		Matrix<10, 10> m1;
+		m1.identity();
+		Matrix<10, 10> m2;
+		m2.identity();
+		Vector<10> v1;
+		v1.zero();
+		TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
+		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
+		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
+	}
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index df382e2c6..8ab8fa2d6 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -81,7 +81,7 @@ int test_mixer(int argc, char *argv[])
 
 	warnx("testing mixer");
 
-	char *filename = "/etc/mixers/IO_pass.mix";
+	const char *filename = "/etc/mixers/IO_pass.mix";
 
 	if (argc > 1)
 		filename = argv[1];
@@ -100,8 +100,6 @@ int test_mixer(int argc, char *argv[])
 	 * e.g. on PX4IO.
 	 */
 
-	unsigned nused = 0;
-
 	const unsigned chunk_size = 64;
 
 	MixerGroup mixer_group(mixer_callback, 0);
@@ -124,7 +122,6 @@ int test_mixer(int argc, char *argv[])
 		return 1;
 
 	/* FIRST mark the mixer as invalid */
-	bool mixer_ok = false;
 	/* THEN actually delete it */
 	mixer_group.reset();
 	char mixer_text[256];		/* large enough for one mixer */
@@ -140,7 +137,6 @@ int test_mixer(int argc, char *argv[])
 
 		/* check for overflow - this would be really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
-			bool mixer_ok = false;
 			return 1;
 		}
 
@@ -156,15 +152,6 @@ int test_mixer(int argc, char *argv[])
 
 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
-
-			/* only set mixer ok if no residual is left over */
-			if (resid == 0) {
-				mixer_ok = true;
-			} else {
-				/* not yet reached the end of the mixer, set as not ok */
-				mixer_ok = false;
-			}
-
 			warnx("used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
@@ -192,7 +179,7 @@ int test_mixer(int argc, char *argv[])
 	should_arm = true;
 
 	/* run through arming phase */
-	for (int i = 0; i < output_max; i++) {
+	for (unsigned i = 0; i < output_max; i++) {
 		actuator_controls[i] = 0.1f;
 		r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
 		r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
@@ -213,7 +200,7 @@ int test_mixer(int argc, char *argv[])
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (int i = 0; i < mixed; i++)
+		for (unsigned i = 0; i < mixed; i++)
 		{
 			/* check mixed outputs to be zero during init phase */
 			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
@@ -228,7 +215,7 @@ int test_mixer(int argc, char *argv[])
 				return 1;
 			}
 
-			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]);
 		}
 		usleep(sleep_quantum_us);
 		sleepcount++;
@@ -244,7 +231,7 @@ int test_mixer(int argc, char *argv[])
 
 	for (int j = -jmax; j <= jmax; j++) {
 
-		for (int i = 0; i < output_max; i++) {
+		for (unsigned i = 0; i < output_max; i++) {
 			actuator_controls[i] = j/10.0f + 0.1f * i;
 			r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
 			r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
@@ -257,11 +244,11 @@ int test_mixer(int argc, char *argv[])
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		warnx("mixed %d outputs (max %d)", mixed, output_max);
-		for (int i = 0; i < mixed; i++)
+		for (unsigned i = 0; i < mixed; i++)
 		{
 			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
 			if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
-				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
 				warnx("mixer violated predicted value");
 				return 1;
 			}
@@ -282,7 +269,7 @@ int test_mixer(int argc, char *argv[])
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (int i = 0; i < mixed; i++)
+		for (unsigned i = 0; i < mixed; i++)
 		{
 			/* check mixed outputs to be zero during init phase */
 			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@@ -316,7 +303,7 @@ int test_mixer(int argc, char *argv[])
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (int i = 0; i < mixed; i++)
+		for (unsigned i = 0; i < mixed; i++)
 		{
 			/* predict value */
 			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
@@ -333,7 +320,7 @@ int test_mixer(int argc, char *argv[])
 			/* check post ramp phase */
 			if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
 				fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
-				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
 				warnx("mixer violated predicted value");
 				return 1;
 			}
@@ -372,6 +359,7 @@ int test_mixer(int argc, char *argv[])
 	}
 
 	warnx("SUCCESS: No errors in mixer test");
+	return 0;
 }
 
 static int
diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c
index 44e34d9ef..4b6303cfb 100644
--- a/src/systemcmds/tests/test_mount.c
+++ b/src/systemcmds/tests/test_mount.c
@@ -141,8 +141,8 @@ test_mount(int argc, char *argv[])
 			/* announce mode switch */
 			if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
 				warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
-				fsync(stdout);
-				fsync(stderr);
+				fsync(fileno(stdout));
+				fsync(fileno(stderr));
 				usleep(20000);
 			}
 
@@ -162,7 +162,7 @@ test_mount(int argc, char *argv[])
 	}
 
 	char buf[64];
-	int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
+	(void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
 	lseek(cmd_fd, 0, SEEK_SET);
 	write(cmd_fd, buf, strlen(buf) + 1);
 	fsync(cmd_fd);
@@ -174,8 +174,8 @@ test_mount(int argc, char *argv[])
 
 		printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
 		printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
-		fsync(stdout);
-		fsync(stderr);
+		fsync(fileno(stdout));
+		fsync(fileno(stderr));
 		usleep(50000);
 
 		for (unsigned a = 0; a < alignments; a++) {
@@ -185,22 +185,20 @@ test_mount(int argc, char *argv[])
 			uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 
 			/* fill write buffer with known values */
-			for (int i = 0; i < sizeof(write_buf); i++) {
+			for (unsigned i = 0; i < sizeof(write_buf); i++) {
 				/* this will wrap, but we just need a known value with spacing */
 				write_buf[i] = i+11;
 			}
 
 			uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
-			hrt_abstime start, end;
 
 			int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 
-			start = hrt_absolute_time();
 			for (unsigned i = 0; i < iterations; i++) {
 
 				int wret = write(fd, write_buf + a, chunk_sizes[c]);
 
-				if (wret != chunk_sizes[c]) {
+				if (wret != (int)chunk_sizes[c]) {
 					warn("WRITE ERROR!");
 
 					if ((0x3 & (uintptr_t)(write_buf + a)))
@@ -214,8 +212,8 @@ test_mount(int argc, char *argv[])
 					fsync(fd);
 				} else {
 					printf("#");
-					fsync(stdout);
-					fsync(stderr);
+					fsync(fileno(stdout));
+					fsync(fileno(stderr));
 				}
 			}
 
@@ -224,12 +222,10 @@ test_mount(int argc, char *argv[])
 			}
 
 			printf(".");
-			fsync(stdout);
-			fsync(stderr);
+			fsync(fileno(stdout));
+			fsync(fileno(stderr));
 			usleep(200000);
 
-			end = hrt_absolute_time();
-
 			close(fd);
 			fd = open("/fs/microsd/testfile", O_RDONLY);
 
@@ -237,7 +233,7 @@ test_mount(int argc, char *argv[])
 			for (unsigned i = 0; i < iterations; i++) {
 				int rret = read(fd, read_buf, chunk_sizes[c]);
 
-				if (rret != chunk_sizes[c]) {
+				if (rret != (int)chunk_sizes[c]) {
 					warnx("READ ERROR!");
 					return 1;
 				}
@@ -245,7 +241,7 @@ test_mount(int argc, char *argv[])
 				/* compare value */
 				bool compare_ok = true;
 
-				for (int j = 0; j < chunk_sizes[c]; j++) {
+				for (unsigned j = 0; j < chunk_sizes[c]; j++) {
 					if (read_buf[j] != write_buf[j + a]) {
 						warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
 						compare_ok = false;
@@ -271,16 +267,16 @@ test_mount(int argc, char *argv[])
 		}
 	}
 
-	fsync(stdout);
-	fsync(stderr);
+	fsync(fileno(stdout));
+	fsync(fileno(stderr));
 	usleep(20000);
 
 
 
 	/* we always reboot for the next test if we get here */
 	warnx("Iteration done, rebooting..");
-	fsync(stdout);
-	fsync(stderr);
+	fsync(fileno(stdout));
+	fsync(fileno(stderr));
 	usleep(50000);
 	systemreset(false);
 
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
index bac9efbdb..43231ccad 100644
--- a/src/systemcmds/tests/test_mtd.c
+++ b/src/systemcmds/tests/test_mtd.c
@@ -57,6 +57,8 @@
 #define PARAM_FILE_NAME "/fs/mtd_params"
 
 static int check_user_abort(int fd);
+static void print_fail(void);
+static void print_success(void);
 
 int check_user_abort(int fd) {
 	/* check if user wants to abort */
@@ -126,7 +128,7 @@ test_mtd(int argc, char *argv[])
 		uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
 
 		/* fill write buffer with known values */
-		for (int i = 0; i < sizeof(write_buf); i++) {
+		for (unsigned i = 0; i < sizeof(write_buf); i++) {
 			/* this will wrap, but we just need a known value with spacing */
 			write_buf[i] = i+11;
 		}
@@ -137,11 +139,14 @@ test_mtd(int argc, char *argv[])
 		int fd = open(PARAM_FILE_NAME, O_RDONLY);
 		int rret = read(fd, read_buf, chunk_sizes[c]);
 		close(fd);
+		if (rret <= 0) {
+			err(1, "read error");
+		}
 
 		fd = open(PARAM_FILE_NAME, O_WRONLY);
 
 		printf("printing 2 percent of the first chunk:\n");
-		for (int i = 0; i < sizeof(read_buf) / 50; i++) {
+		for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) {
 			printf("%02X", read_buf[i]);
 		}
 		printf("\n");
@@ -166,14 +171,16 @@ test_mtd(int argc, char *argv[])
 		}
 		end = hrt_absolute_time();
 
+		warnx("write took %llu us", (end - start));
+
 		close(fd);
 		fd = open(PARAM_FILE_NAME, O_RDONLY);
 
 		/* read back data for validation */
 		for (unsigned i = 0; i < iterations; i++) {
-			int rret = read(fd, read_buf, chunk_sizes[c]);
+			int rret2 = read(fd, read_buf, chunk_sizes[c]);
 
-			if (rret != chunk_sizes[c]) {
+			if (rret2 != (int)chunk_sizes[c]) {
 				warnx("READ ERROR!");
 				print_fail();
 				return 1;
@@ -182,7 +189,7 @@ test_mtd(int argc, char *argv[])
 			/* compare value */
 			bool compare_ok = true;
 
-			for (int j = 0; j < chunk_sizes[c]; j++) {
+			for (unsigned j = 0; j < chunk_sizes[c]; j++) {
 				if (read_buf[j] != write_buf[j]) {
 					warnx("COMPARISON ERROR: byte %d", j);
 					print_fail();
@@ -211,7 +218,7 @@ test_mtd(int argc, char *argv[])
 	char ffbuf[64];
 	memset(ffbuf, 0xFF, sizeof(ffbuf));
 	int fd = open(PARAM_FILE_NAME, O_WRONLY);
-	for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
+	for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) {
 		int ret = write(fd, ffbuf, sizeof(ffbuf));
 
 		if (ret != sizeof(ffbuf)) {
diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c
index b9041b013..addd57bea 100644
--- a/src/systemcmds/tests/test_ppm_loopback.c
+++ b/src/systemcmds/tests/test_ppm_loopback.c
@@ -65,7 +65,6 @@ int test_ppm_loopback(int argc, char *argv[])
 	int _rc_sub = orb_subscribe(ORB_ID(input_rc));
 
 	int servo_fd, result;
-	servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
 	servo_position_t pos;
 
 	servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
index 57c0e7f4c..c9f9ef439 100644
--- a/src/systemcmds/tests/test_rc.c
+++ b/src/systemcmds/tests/test_rc.c
@@ -52,6 +52,7 @@
 #include <arch/board/board.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_rc_input.h>
+#include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 
 #include "tests.h"
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index 096106ff3..a4f17eebd 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -139,7 +139,14 @@ accel(int argc, char *argv[])
 	}
 
 	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
-		warnx("ACCEL1 acceleration values out of range!");
+		warnx("ACCEL acceleration values out of range!");
+		return ERROR;
+	}
+
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len < 8.0f || len > 12.0f) {
+		warnx("ACCEL scale error!");
 		return ERROR;
 	}
 
@@ -186,6 +193,13 @@ accel1(int argc, char *argv[])
 		return ERROR;
 	}
 
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len < 8.0f || len > 12.0f) {
+		warnx("ACCEL1 scale error!");
+		return ERROR;
+	}
+
 	/* Let user know everything is ok */
 	printf("\tOK: ACCEL1 passed all tests successfully\n");
 
@@ -225,6 +239,13 @@ gyro(int argc, char *argv[])
 		printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len > 0.3f) {
+		warnx("GYRO scale error!");
+		return ERROR;
+	}
+
 	/* Let user know everything is ok */
 	printf("\tOK: GYRO passed all tests successfully\n");
 	close(fd);
@@ -263,6 +284,13 @@ gyro1(int argc, char *argv[])
 		printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len > 0.3f) {
+		warnx("GYRO1 scale error!");
+		return ERROR;
+	}
+
 	/* Let user know everything is ok */
 	printf("\tOK: GYRO1 passed all tests successfully\n");
 	close(fd);
@@ -301,6 +329,13 @@ mag(int argc, char *argv[])
 		printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len < 1.0f || len > 3.0f) {
+		warnx("MAG scale error!");
+		return ERROR;
+	}
+
 	/* Let user know everything is ok */
 	printf("\tOK: MAG passed all tests successfully\n");
 	close(fd);
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
index ef8512bf5..9c6951ca2 100644
--- a/src/systemcmds/tests/test_servo.c
+++ b/src/systemcmds/tests/test_servo.c
@@ -51,6 +51,7 @@
 
 #include <arch/board/board.h>
 #include <drivers/drv_pwm_output.h>
+#include <systemlib/err.h>
 
 #include <nuttx/spi.h>
 
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index 82de05dff..ad55e1410 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -107,11 +107,13 @@ extern int	test_jig_voltages(int argc, char *argv[]);
 extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
+extern int	test_file2(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);
 extern int	test_mount(int argc, char *argv[]);
 extern int	test_mtd(int argc, char *argv[]);
+extern int	test_mathlib(int argc, char *argv[]);
 
 __END_DECLS
 
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 77a4df618..e3f26924f 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -104,11 +104,13 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"file2",		test_file2,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mtd",			test_mtd,	0},
+	{"mathlib",		test_mathlib,	0},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };
@@ -233,7 +235,7 @@ test_perf(int argc, char *argv[])
 	printf("perf: expect count of 1\n");
 	perf_print_counter(ec);
 	printf("perf: expect at least two counters\n");
-	perf_print_all();
+	perf_print_all(0);
 
 	perf_free(cc);
 	perf_free(ec);
diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk
index 9239aafc3..548a09f85 100644
--- a/src/systemcmds/top/module.mk
+++ b/src/systemcmds/top/module.mk
@@ -38,7 +38,7 @@
 MODULE_COMMAND	 = top
 SRCS		 = top.c
 
-MODULE_STACKSIZE = 3000
+MODULE_STACKSIZE = 1700
 
 MAXOPTIMIZATION	 = -Os
 
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index 1ca3fc928..37e913040 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -233,8 +233,8 @@ top_main(void)
 					   system_load.tasks[i].tcb->pid,
 					   CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
 					   (system_load.tasks[i].total_runtime / 1000),
-					   (int)(curr_loads[i] * 100),
-					   (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
+					   (int)(curr_loads[i] * 100.0f),
+					   (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000),
 					   stack_size - stack_free,
 					   stack_size,
 					   system_load.tasks[i].tcb->sched_priority,
diff --git a/src/systemcmds/ver/module.mk b/src/systemcmds/ver/module.mk
new file mode 100644
index 000000000..2eeb80b61
--- /dev/null
+++ b/src/systemcmds/ver/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# "version" nsh-command displays version infromation for hw,sw, gcc,build etc
+# can be also included and used in own code via "ver.h"
+#
+
+MODULE_COMMAND	 	= ver
+SRCS		 		= ver.c
+
+MODULE_STACKSIZE 	= 1024
+
+MAXOPTIMIZATION	 	= -Os
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
new file mode 100644
index 000000000..9ae080ee2
--- /dev/null
+++ b/src/systemcmds/ver/ver.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+*
+* 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 ver.c
+*
+* Version command, unifies way of showing versions of HW, SW, Build, GCC
+* In case you want to add new version just extend version_main function
+*
+* @author Vladimir Kulla <ufon@kullaonline.net>
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <version/version.h>
+#include <systemlib/err.h>
+
+// string constants for version commands
+static const char sz_ver_hw_str[] 	= "hw";
+static const char sz_ver_hwcmp_str[] = "hwcmp";
+static const char sz_ver_git_str[] 	= "git";
+static const char sz_ver_bdate_str[] = "bdate";
+static const char sz_ver_gcc_str[] 	= "gcc";
+static const char sz_ver_all_str[] 	= "all";
+
+static void usage(const char *reason)
+{
+	if (reason != NULL) {
+		printf("%s\n", reason);
+	}
+
+	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n");
+}
+
+__EXPORT int ver_main(int argc, char *argv[]);
+
+int ver_main(int argc, char *argv[])
+{
+	int ret = 1;	//defaults to an error
+
+	// first check if there are at least 2 params
+	if (argc >= 2) {
+		if (argv[1] != NULL) {
+			if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
+				printf("%s\n", HW_ARCH);
+				ret = 0;
+
+			} else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
+				if (argc >= 3 && argv[2] != NULL) {
+					// compare 3rd parameter with HW_ARCH string, in case of match, return 0
+					ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
+
+					if (ret == 0) {
+						printf("ver hwcmp match: %s\n", HW_ARCH);
+					}
+
+				} else {
+					errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
+				}
+
+			} else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
+				printf("%s\n", FW_GIT);
+				ret = 0;
+
+			} else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
+				printf("%s %s\n", __DATE__, __TIME__);
+				ret = 0;
+
+			} else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
+				printf("%s\n", __VERSION__);
+				ret = 0;
+
+			} else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) {
+				printf("HW arch: %s\n", HW_ARCH);
+				printf("Build datetime: %s %s\n", __DATE__, __TIME__);
+				printf("FW git-hash: %s\n", FW_GIT);
+				printf("GCC toolchain: %s\n", __VERSION__);
+				ret = 0;
+
+			} else {
+				errx(1, "unknown command.\n");
+			}
+
+		} else {
+			usage("Error, input parameter NULL.\n");
+		}
+
+	} else {
+		usage("Error, not enough parameters.");
+	}
+
+	return ret;
+}